public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, Vector3d centerOfMassOffset) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), centerOfMassOffset, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
public static double computeSubTreeMass(RigidBody rootBody) { RigidBodyInertia inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (InverseDynamicsJoint childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
public ConstrainedCentroidalMomentumMatrixCalculator(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame centerOfMassFrame, DenseMatrix64F selectionMatrix) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootJoint.getSuccessor(), centerOfMassFrame); this.selectionMatrix = selectionMatrix; }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); setPosition(sixDoFOriginalJoint.jointTranslation); setRotation(sixDoFOriginalJoint.jointRotation); jointTwist.setAngularPart(sixDoFOriginalJoint.jointTwist.getAngularPart()); jointTwist.setLinearPart(sixDoFOriginalJoint.jointTwist.getLinearPart()); jointAcceleration.setAngularPart(sixDoFOriginalJoint.jointAcceleration.getAngularPart()); jointAcceleration.setLinearPart(sixDoFOriginalJoint.jointAcceleration.getLinearPart()); }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { checksum.update(jointTranslation); checksum.update(jointRotation); checksum.update(jointTwist.getAngularPart()); checksum.update(jointTwist.getLinearPart()); checksum.update(jointAcceleration.getAngularPart()); checksum.update(jointAcceleration.getLinearPart()); }
@Override public void getSuccessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.changeFrame(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); }
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
@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 setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
private void setOffDiagonalTerms(int i) { int parentIndex; int j = i; while (isValidParentIndex(parentIndex = parentMap[j])) { ReferenceFrame parentFrame = allRigidBodiesInOrder[parentIndex].getInertia().getExpressedInFrame(); changeFrameOfMomenta(parentFrame); j = parentIndex; GeometricJacobian motionSubspace = allRigidBodiesInOrder[j].getParentJoint().getMotionSubspace(); setMassMatrixPart(i, j, motionSubspace); } }
public ConstrainedCenterOfMassJacobianCalculator(FloatingInverseDynamicsJoint rootJoint) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centerOfMassJacobian = new CenterOfMassJacobian(rootJoint.getSuccessor()); }
public void setRandomPositionsAndVelocities(Random random) { ScrewTestTools.setRandomPositionAndOrientation(getRootJoint(), random); ScrewTestTools.setRandomVelocity(getRootJoint(), random); ScrewTestTools.setRandomPositions(getRevoluteJoints(), random); setRandomVelocities(getRevoluteJoints(), random); getElevator().updateFramesRecursively(); }
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); }
@Override public void getPredecessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.invert(); twistToPack.changeFrame(predecessorFrame); }
@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); }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, RigidBodyTransform inertiaPose) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), inertiaPose, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
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); }