@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 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 getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack) { getDesiredJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.invert(); accelerationToPack.changeFrameNoRelativeMotion(predecessorFrame); }
@Override public void compute() { if (!isEnabled()) return; yoDesiredPosition.getFrameTupleIncludingFrame(tempPosition); yoDesiredLinearVelocity.getFrameTupleIncludingFrame(tempLinearVelocity); yoFeedForwardLinearAcceleration.getFrameTupleIncludingFrame(feedForwardLinearAcceleration); yoDesiredOrientation.getFrameOrientationIncludingFrame(tempOrientation); yoDesiredAngularVelocity.getFrameTupleIncludingFrame(tempAngularVelocity); yoFeedForwardAngularAcceleration.getFrameTupleIncludingFrame(feedForwardAngularAcceleration); accelerationControlModule.doPositionControl(tempPosition, tempOrientation, tempLinearVelocity, tempAngularVelocity, feedForwardLinearAcceleration, feedForwardAngularAcceleration, base); accelerationControlModule.getAcceleration(desiredSpatialAcceleration); desiredSpatialAcceleration.changeBodyFrameNoRelativeAcceleration(endEffectorFrame); desiredSpatialAcceleration.changeFrameNoRelativeMotion(endEffectorFrame); updatePositionVisualization(); updateOrientationVisualization(); output.setSpatialAcceleration(desiredSpatialAcceleration); }
@Override public void doSpecificAction() { if (partialFootholdControlModule != null) { footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop); YoPlaneContactState contactState = momentumBasedController.getContactState(contactableFoot); boolean contactStateHasChanged = partialFootholdControlModule.applyShrunkPolygon(contactState); if (contactStateHasChanged) contactState.notifyContactStateHasChanged(); } footAcceleration.setToZero(contactableFoot.getFrameAfterParentJoint(), rootBody.getBodyFixedFrame(), contactableFoot.getFrameAfterParentJoint()); ReferenceFrame bodyFixedFrame = contactableFoot.getRigidBody().getBodyFixedFrame(); footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame); footAcceleration.changeFrameNoRelativeMotion(bodyFixedFrame); spatialAccelerationCommand.setSpatialAcceleration(footAcceleration); }
accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame);
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); }
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); }
footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame); footAcceleration.changeFrameNoRelativeMotion(bodyFixedFrame); spatialAccelerationCommand.setSpatialAcceleration(footAcceleration);