@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame); twistToPack.setAngularPart(jointAngularVelocity.getVector()); }
private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBody body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); twistCalculator.getTwistOfBody(tempTwist, body); if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(tempAcceleration, body); body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); } }
@Override public void updateRootJointOrientationAndAngularVelocity() { yoRootJointFrameOrientation.getQuaternion(rootJointOrientation); rootJoint.setRotation(rootJointOrientation); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
public void setAngularVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredAngularVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredAngularVelocity.getReferenceFrame()); spatialVelocity.setAngularPart(desiredAngularVelocity.getVector()); setSelectionMatrixForAngularControl(); }
protected void putYoValuesIntoTwist() { twist.setToZero(bodyFrame, baseFrame, expressedInFrame); twist.setLinearPart(linearPart.getFrameTuple()); twist.setAngularPart(angularPart.getFrameTuple()); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { int index = rowStart; double qdRot = matrix.get(index++, 0); double xd = matrix.get(index++, 0); double zd = matrix.get(index++, 0); jointTwist.setToZero(); jointTwist.setAngularPartY(qdRot); jointTwist.setLinearPart(xd, 0.0, zd); }
public void setLinearVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredLinearVelocity.getReferenceFrame()); spatialVelocity.setLinearPart(desiredLinearVelocity.getVector()); spatialVelocity.changeFrame(bodyFrame); setSelectionMatrixForLinearControl(); }
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); }
public void updateForFrozenState() { // Keep setting the position so the localization updater works properly. yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); // Set the rootJoint twist to zero. rootJoint.getJointTwist(rootJointTwist); rootJointTwist.setToZero(); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); updateCoMState(); }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
/** * Computes using Twists, ignores linear part */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); }
/** * Computes linear portion of twist to pack */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
@Override public void updateForFrozenState() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); yoRootJointFrameQuaternion.getYawPitchRoll(lastComputedYawPitchRoll); double currentYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); double yawDifference = AngleTools.computeAngleDifferenceMinusPiToPi(lastComputedYawPitchRoll[0], currentYaw); rootJointYawOffsetFromFrozenState.set(yawDifference); rotationFrozenOffset.rotZ(yawDifference); // Keep setting the orientation so that the localization updater works properly. yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); updateViz(); }
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
/** * This method provides a twist feedback controller, intended for spatial velocity control. * @param twistToPack twist to return * @param desiredPose desired pose that we want to achieve. * @param desiredTwist feed forward twist from a reference trajectory */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
public void computeJacobianDerivativeTerm(GeometricJacobian jacobian, SpatialAccelerationVector accelerationToPack) twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector();
public CentroidalMomentumRateADotVTerm(RigidBody rootBody, ReferenceFrame centerOfMassFrame, CentroidalMomentumMatrix centroidalMomentumMatrix, double robotMass, DenseMatrix64F v) { this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody); this.robotMass = robotMass; this.centerOfMassFrame = centerOfMassFrame; this.rootBody = rootBody; this.v = v; this.twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), rootBody); this.centroidalMomentumMatrix = centroidalMomentumMatrix; this.rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame()); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false); this.comTwist = new Twist(centerOfMassFrame, rootBody.getBodyFixedFrame(), centerOfMassFrame); comTwist.setToZero(); this.comVelocityVector = new Vector3d(); this.aDotV = new DenseMatrix64F(6, 1); this.tempSpatialAcceleration = new SpatialAccelerationVector(); this.tempMomentum = new Momentum(); this.tempTwist = new Twist(); this.tempCoMTwist = new Twist(); this.tempVector = new Vector3d(0, 0, 0); this.leftSide = new Momentum(centerOfMassFrame); }
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); }
comTwist.setToZero(); this.tempVector = new Vector3d(0,0,0); this.aDotV = new DenseMatrix64F(6,1);