public PlanarJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
public SixDoFJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
private void populateMapsAndLists() { accelerations.put(rootBody, new SpatialAccelerationVector()); for (InverseDynamicsJoint joint : allJoints) { if (joint.getSuccessor() != null) { accelerations.put(joint.getSuccessor(), new SpatialAccelerationVector()); } } } }
public static void copyDesiredAccelerationToActual(SixDoFJoint rootJoint) { SpatialAccelerationVector rootJointAcceleration = new SpatialAccelerationVector(); rootJoint.getDesiredJointAcceleration(rootJointAcceleration); rootJoint.setAcceleration(rootJointAcceleration); }
public RigidBodySpatialAccelerationControlModule(String namePrefix, TwistCalculator twistCalculator, RigidBody endEffector, ReferenceFrame endEffectorFrame, double dt, YoSE3PIDGainsInterface taskspaceGains, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.twistCalculator = twistCalculator; this.endEffector = endEffector; this.endEffectorFrame = endEffectorFrame; this.se3pdController = new SE3PIDController(namePrefix, endEffectorFrame, dt, taskspaceGains, registry); this.acceleration = new SpatialAccelerationVector(); parentRegistry.addChild(registry); }
public static SpatialAccelerationVector createGravitationalSpatialAcceleration(RigidBody rootBody, double gravity) { Vector3d gravitationalAcceleration = new Vector3d(0.0, 0.0, gravity); Vector3d zero = new Vector3d(); SpatialAccelerationVector rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame(), gravitationalAcceleration, zero); return rootAcceleration; }
this.rootAcceleration = new SpatialAccelerationVector(rootAcceleration); this.twistCalculator = twistCalculator; this.doVelocityTerms = doVelocityTerms;
public static void setRandomAcceleration(SixDoFJoint rootJoint, Random random) { SpatialAccelerationVector jointAcceleration = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(jointAcceleration); jointAcceleration.setAngularPart(RandomTools.generateRandomVector(random)); jointAcceleration.setLinearPart(RandomTools.generateRandomVector(random)); rootJoint.setAcceleration(jointAcceleration); }
public static void integrateAccelerations(SixDoFJoint rootJoint, double dt) { SpatialAccelerationVector deltaTwist = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(deltaTwist); deltaTwist.scale(dt); Twist rootJointTwist = new Twist(); rootJoint.getJointTwist(rootJointTwist); rootJointTwist.angularPart.add(deltaTwist.angularPart); rootJointTwist.linearPart.add(deltaTwist.linearPart); rootJoint.setJointTwist(rootJointTwist); }
@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); }
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); }
this.tempSpatialAcceleration = new SpatialAccelerationVector(); this.rootAcceleration = new SpatialAccelerationVector(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame()); this.spatialAccelerationCalculator = new SpatialAccelerationCalculator(rootBody, ReferenceFrame.getWorldFrame(), this.rootAcceleration, this.twistCalculator, true, false, false);
public LoadBearingHandControlState(String namePrefix, HandControlMode stateEnum, RobotSide robotSide, HighLevelHumanoidControllerToolbox momentumBasedController, RigidBody elevator, RigidBody endEffector, YoVariableRegistry parentRegistry) { super(stateEnum); name = namePrefix + FormattingTools.underscoredToCamelCase(this.getStateEnum().toString(), true) + "State"; registry = new YoVariableRegistry(name); spatialAccelerationCommand.set(elevator, endEffector); spatialAccelerationCommand.setSelectionMatrixToIdentity(); this.momentumBasedController = momentumBasedController; parentRegistry.addChild(registry); coefficientOfFriction = new DoubleYoVariable(name + "CoefficientOfFriction", registry); handAcceleration = new SpatialAccelerationVector(endEffector.getBodyFixedFrame(), elevator.getBodyFixedFrame(), endEffector.getBodyFixedFrame()); elevatorFrame = elevator.getBodyFixedFrame(); handFrame = endEffector.getBodyFixedFrame(); SideDependentList<ContactablePlaneBody> contactableHands = momentumBasedController.getContactableHands(); if (contactableHands != null) { handPalm = contactableHands.get(robotSide); contactNormal = new FrameVector(); contactNormal.setToNaN(); momentumBasedController.setPlaneContactStateFree(handPalm); } else { handPalm = null; contactNormal = null; } }
public void compute(FrameVector desiredAngularAcceleration) { int vectorSize = SpatialMotionVector.SIZE / 2; DenseMatrix64F biasedAccelerationMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F jacobianDerivativeTermMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F angularJacobian = new DenseMatrix64F(SpatialMotionVector.SIZE / 2, jacobian.getNumberOfColumns()); DenseMatrix64F jointAccelerations = new DenseMatrix64F(angularJacobian.getNumCols(), 1); desiredAngularAcceleration.changeFrame(jacobian.getJacobianFrame()); desiredAngularAcceleration.getInMatrixColumn(biasedAccelerationMatrix, 0); CommonOps.scale(sign, biasedAccelerationMatrix); jacobian.compute(); SpatialAccelerationVector jacobianDerivativeTerm = new SpatialAccelerationVector(); jointAccelerationCalculator.computeJacobianDerivativeTerm(jacobianDerivativeTerm); MatrixTools.setDenseMatrixFromTuple3d(jacobianDerivativeTermMatrix, jacobianDerivativeTerm.getAngularPartCopy(), 0, 0); CommonOps.subtractEquals(biasedAccelerationMatrix, jacobianDerivativeTermMatrix); SubmatrixOps.setSubMatrix(jacobian.getJacobianMatrix(), angularJacobian, 0, 0, 0, 0, angularJacobian.getNumRows(), angularJacobian.getNumCols()); CommonOps.solve(angularJacobian, biasedAccelerationMatrix, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }