private void computeSuccessorAcceleration(InverseDynamicsJoint joint) { RigidBody predecessor = joint.getPredecessor(); RigidBody successor = joint.getSuccessor(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); joint.getPredecessorTwist(tempJointTwist); if (!doVelocityTerms) tempJointTwist.setToZero(); if (useDesireds) joint.getDesiredSuccessorAcceleration(tempJointAcceleration); else joint.getSuccessorAcceleration(tempJointAcceleration); if (!doAccelerationTerms) tempJointAcceleration.setToZero(); twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor); if (!doVelocityTerms) tempTwistFromWorld.setToZero(); SpatialAccelerationVector successorAcceleration = accelerations.get(successor); successorAcceleration.set(accelerations.get(predecessor)); successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld); successorAcceleration.add(tempJointAcceleration); }
if (currentBody == joint.getPredecessor()) joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor();
public void computeJacobianDerivativeTerm(SpatialAccelerationVector accelerationToPack) { ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame(); twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); for (int i = jacobian.getJointsInOrder().length - 1; i >= 0; i--) { twistOfCurrentWithRespectToBase.changeFrame(currentBody.getBodyFixedFrame()); InverseDynamicsJoint joint = jacobian.getJointsInOrder()[i]; if (currentBody == joint.getPredecessor()) { joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor(); } else { joint.getSuccessorTwist(jointTwist); currentBody = joint.getPredecessor(); } zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack); accelerationToPack.set(zeroAcceleration); jointTwist.invert(); twistOfCurrentWithRespectToBase.add(jointTwist); } accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame); }