public void setLinearAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredLinearAcceleration.getReferenceFrame()); spatialAcceleration.setLinearPart(desiredLinearAcceleration.getVector()); spatialAcceleration.changeFrameNoRelativeMotion(bodyFrame); }
@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); }
@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 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 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); }
@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); }
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); }
private void transformDesiredsFromSoleFrameToControlFrame() { desiredSoleFrame.setPoseAndUpdate(desiredPosition, desiredOrientation); // change pose desiredPose.setToZero(desiredControlFrame); desiredPose.changeFrame(worldFrame); desiredPose.getPosition(desiredPosition.getPoint()); desiredPose.getOrientation(desiredOrientation.getQuaternion()); // change twist desiredLinearVelocity.changeFrame(desiredSoleFrame); desiredAngularVelocity.changeFrame(desiredSoleFrame); desiredTwist.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearVelocity, desiredAngularVelocity); desiredTwist.changeFrame(desiredControlFrame); desiredTwist.getLinearPart(desiredLinearVelocity); desiredTwist.getAngularPart(desiredAngularVelocity); desiredLinearVelocity.changeFrame(worldFrame); desiredAngularVelocity.changeFrame(worldFrame); // change spatial acceleration desiredLinearAcceleration.changeFrame(desiredSoleFrame); desiredAngularAcceleration.changeFrame(desiredSoleFrame); desiredSpatialAcceleration.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearAcceleration, desiredAngularAcceleration); desiredSpatialAcceleration.changeFrameNoRelativeMotion(desiredControlFrame); desiredSpatialAcceleration.getLinearPart(desiredLinearAcceleration); desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration); desiredLinearAcceleration.changeFrame(worldFrame); desiredAngularAcceleration.changeFrame(worldFrame); }
tempRootToEstimationAcceleration.changeFrameNoRelativeMotion(rootJoint.getFrameAfterJoint()); tempRootToEstimationAcceleration.getAngularPart(tempRootToEstimationAngularAcceleration); tempRootToEstimationAngularAcceleration.changeFrame(estimationFrame);
ReferenceFrame bodyFixedFrame = contactableFoot.getRigidBody().getBodyFixedFrame(); footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame); footAcceleration.changeFrameNoRelativeMotion(bodyFixedFrame); spatialAccelerationCommand.setSpatialAcceleration(footAcceleration);
spatialAccelerationOfMeasurementLink.changeFrame(estimationLink.getBodyFixedFrame(), twistOfMeasurementFrameWithRespectToEstimation, twistOfMeasurementLink); spatialAccelerationOfMeasurementLink.changeFrameNoRelativeMotion(estimationFrame); twistOfMeasurementLink.changeFrame(estimationFrame);