@Override public void getTauMatrix(DenseMatrix64F matrix) { matrix.set(0, 0, successorWrench.getAngularPartY()); matrix.set(1, 0, successorWrench.getLinearPartX()); matrix.set(2, 0, successorWrench.getLinearPartZ()); }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.setToZero(successorWrench.getBodyFrame(), successorWrench.getExpressedInFrame()); wrenchToPack.setAngularPartY(successorWrench.getAngularPartY()); wrenchToPack.setLinearPartX(successorWrench.getLinearPartX()); wrenchToPack.setLinearPartZ(successorWrench.getLinearPartZ()); }
protected void getYoValuesFromWrench() { linearPart.set(wrench.getExpressedInFrame(), wrench.getLinearPartX(), wrench.getLinearPartY(), wrench.getLinearPartZ()); angularPart.set(wrench.getExpressedInFrame(), wrench.getAngularPartX(), wrench.getAngularPartY(), wrench.getAngularPartZ()); }
public void update(Wrench sensorWrench) { sensorWrench.changeFrame(world); yoSensorForce.set(sensorWrench.getExpressedInFrame(), sensorWrench.getLinearPartX(), sensorWrench.getLinearPartY(), sensorWrench.getLinearPartZ()); yoSensorTorque.set(sensorWrench.getExpressedInFrame(), sensorWrench.getAngularPartX(), sensorWrench.getAngularPartY(), sensorWrench.getAngularPartZ()); if (addSimulatedSensorNoise.getBooleanValue()) { double amp = 0.1; double bias = 0.25; yoSensorForce.add(amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias); yoSensorTorque.add(amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias); } updateSensorPosition(); updateCenterOfMass(); yoSensorToDistalCoMvectorInWorld.sub(distalCoMInWorld, yoSensorPositionInWorld); distalMassWrench.setToZero(world); distalMassWrench.setUsingArm(world, distalMassForceInWorld.getFrameTuple().getVector(), yoSensorToDistalCoMvectorInWorld.getFrameTuple().getVector()); yoSensorForceFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getLinearPartX(), distalMassWrench.getLinearPartY(), distalMassWrench.getLinearPartZ()); yoSensorTorqueFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getAngularPartX(), distalMassWrench.getAngularPartY(), distalMassWrench.getAngularPartZ()); yoSensorForceMassCompensated.sub(yoSensorForce, yoSensorForceFromDistalMass); yoSensorTorqueMassCompensated.sub(yoSensorTorque, yoSensorTorqueFromDistalMass); }
public void setWrench(Wrench jointWrench) { successorWrench.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); successorWrench.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); successorWrench.setAngularPartY(jointWrench.getAngularPartY()); successorWrench.setLinearPartX(jointWrench.getLinearPartX()); successorWrench.setLinearPartZ(jointWrench.getLinearPartZ()); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.setToZero(successor.getBodyFixedFrame(), successorWrench.getExpressedInFrame()); jointWrench.setAngularPartY(successorWrench.getAngularPartY()); jointWrench.setLinearPartX(successorWrench.getLinearPartX()); jointWrench.setLinearPartZ(successorWrench.getLinearPartZ()); jointWrench.changeFrame(successor.getBodyFixedFrame()); }