@Override public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { getJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.changeFrameNoRelativeMotion(successorFrame); }
@Override public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { getDesiredJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.changeFrameNoRelativeMotion(successorFrame); }
@Override public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack) { getDesiredJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.invert(); accelerationToPack.changeFrameNoRelativeMotion(predecessorFrame); }
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
public void control(SpatialAccelerationVector handSpatialAccelerationVector, Wrench toolWrench) { if (!hasBeenInitialized) { update(); initialize(); } update(); toolAcceleration.set(handSpatialAccelerationVector); toolAcceleration.changeFrameNoRelativeMotion(toolJoint.getFrameAfterJoint()); // TODO: Take relative acceleration between uTorsoCoM and elevator in account toolAcceleration.changeBaseFrameNoRelativeAcceleration(elevatorFrame); toolAcceleration.changeBodyFrameNoRelativeAcceleration(toolJoint.getFrameAfterJoint()); toolJoint.setDesiredAcceleration(toolAcceleration); inverseDynamicsCalculator.compute(); inverseDynamicsCalculator.getJointWrench(toolJoint, toolWrench); toolWrench.negate(); toolWrench.changeFrame(handFixedFrame); toolWrench.changeBodyFrameAttachedToSameBody(handFixedFrame); // Visualization temporaryVector.setIncludingFrame(handFixedFrame, toolWrench.getLinearPartX(), toolWrench.getLinearPartY(), toolWrench.getLinearPartZ()); temporaryVector.changeFrame(ReferenceFrame.getWorldFrame()); temporaryVector.scale(0.01); objectForceInWorld.set(temporaryVector); }
@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); }
@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); }