@Override public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitSuccessorAcceleration); accelerationToPack.scale(qddDesired); }
@Override public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitSuccessorAcceleration); accelerationToPack.scale(qdd); }
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitJointAcceleration); accelerationToPack.scale(qdd); }
@Override public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitJointAcceleration); accelerationToPack.scale(qddDesired); }
@Override public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(unitPredecessorAcceleration); accelerationToPack.scale(qddDesired); }
public static void integrateAccelerations(SixDoFJoint rootJoint, double dt) { SpatialAccelerationVector deltaTwist = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(deltaTwist); deltaTwist.scale(dt); Twist rootJointTwist = new Twist(); rootJoint.getJointTwist(rootJointTwist); rootJointTwist.angularPart.add(deltaTwist.angularPart); rootJointTwist.linearPart.add(deltaTwist.linearPart); rootJoint.setJointTwist(rootJointTwist); }