@Override public void getPredecessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.invert(); twistToPack.changeFrame(predecessorFrame); }
accelerationToPack.set(zeroAcceleration); jointTwist.invert(); twistOfCurrentWithRespectToBase.add(jointTwist);
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); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, new Vector3d(), jointAxis.getVector()); unitSuccessorTwist = new Twist(unitJointTwist); unitSuccessorTwist.changeBaseFrameNoRelativeTwist(predecessorFrame); unitSuccessorTwist.changeBodyFrameNoRelativeTwist(successorFrame); unitSuccessorTwist.changeFrame(successorFrame); unitPredecessorTwist = new Twist(unitSuccessorTwist); unitPredecessorTwist.invert(); unitPredecessorTwist.changeFrame(predecessorFrame); unitJointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame, new Vector3d(), jointAxis.getVector()); unitSuccessorAcceleration = new SpatialAccelerationVector(unitJointAcceleration); unitSuccessorAcceleration.changeBaseFrameNoRelativeAcceleration(predecessorFrame); unitSuccessorAcceleration.changeBodyFrameNoRelativeAcceleration(successorFrame); unitSuccessorAcceleration.changeFrameNoRelativeMotion(successorFrame); unitPredecessorAcceleration = new SpatialAccelerationVector(unitSuccessorAcceleration); unitPredecessorAcceleration.invert(); unitPredecessorAcceleration.changeFrameNoRelativeMotion(predecessorFrame); // actually, there is relative motion, but not in the directions that matter setMotionSubspace(unitSuccessorTwist); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; ReferenceFrame predecessorFrame = getPredecessor().getBodyFixedFrame(); ReferenceFrame successorFrame = getSuccessor().getBodyFixedFrame(); unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); unitSuccessorTwist = new Twist(unitJointTwist); unitSuccessorTwist.changeBaseFrameNoRelativeTwist(predecessorFrame); unitSuccessorTwist.changeBodyFrameNoRelativeTwist(successorFrame); unitSuccessorTwist.changeFrame(successorFrame); unitPredecessorTwist = new Twist(unitSuccessorTwist); unitPredecessorTwist.invert(); unitPredecessorTwist.changeFrame(predecessorFrame); unitJointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); unitSuccessorAcceleration = new SpatialAccelerationVector(unitJointAcceleration); unitSuccessorAcceleration.changeBaseFrameNoRelativeAcceleration(predecessorFrame); unitSuccessorAcceleration.changeBodyFrameNoRelativeAcceleration(successorFrame); unitSuccessorAcceleration.changeFrameNoRelativeMotion(successorFrame); unitPredecessorAcceleration = new SpatialAccelerationVector(unitSuccessorAcceleration); unitPredecessorAcceleration.invert(); unitPredecessorAcceleration.changeFrameNoRelativeMotion(predecessorFrame); // actually, there is relative motion, but not in the directions that matter setMotionSubspace(unitSuccessorTwist); }