/** * Packs this rigid-body's center of mass coordinates in the given argument. * <p> * Note that unless modified in runtime, this method outputs {@code (0, 0, 0)} in the * body-fixed-frame. The body-fixed-frame is centered at this rigid-body's center of mass position * by default. * </p> * * @param centerOfMassToPack the {@code FramePoint} in which the center of mass position is stored. * Modified. */ default void getCenterOfMass(FramePoint3DBasics centerOfMassToPack) { centerOfMassToPack.setIncludingFrame(getInertia().getCenterOfMassOffset()); }
/** * Gets the total mass of the multi-body system. * * @return the mass of the multi-body system. */ public double getTotalMass() { if (!isTotalMassUpToDate) { totalMass = 0.0; List<? extends JointReadOnly> jointsToConsider = input.getJointsToConsider(); for (int i = 0; i < jointsToConsider.size(); i++) { totalMass += jointsToConsider.get(i).getSuccessor().getInertia().getMass(); } isTotalMassUpToDate = true; } return totalMass; }
/** * Gets the total mass of the multi-body system. * * @return the mass of the multi-body system. */ public double getTotalMass() { if (!isTotalMassUpToDate) { totalMass = 0.0; List<? extends JointReadOnly> jointsToConsider = input.getJointsToConsider(); for (int i = 0; i < jointsToConsider.size(); i++) { totalMass += jointsToConsider.get(i).getSuccessor().getInertia().getMass(); } isTotalMassUpToDate = true; } return totalMass; }
public RecursionStep(RigidBodyReadOnly rigidBody, int[] jointIndices) { this.rigidBody = rigidBody; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; jacobianJointBlock = null; } else { bodyInertia = new SpatialInertia(rigidBody.getInertia()); jacobianJointBlock = new DenseMatrix64F(3, getJoint().getDegreesOfFreedom()); } }
public IterativeStep(RigidBodyReadOnly rigidBody, int[] jointIndices) { this.rigidBody = rigidBody; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; centroidalMomentumMatrixBlock = null; matrixFrameToBodyFixedFrameTransform = null; } else { bodyInertia = new SpatialInertia(rigidBody.getInertia()); centroidalMomentumMatrixBlock = new DenseMatrix64F(6, getJoint().getDegreesOfFreedom()); matrixFrameToBodyFixedFrameTransform = new RigidBodyTransform(); } }
public CompositeRigidBodyInertia(RigidBodyReadOnly rigidBody, CompositeRigidBodyInertia parent, int[] jointIndices) { this.rigidBody = rigidBody; this.parent = parent; this.jointIndices = jointIndices; if (parent == null) { bodyInertia = null; unitMomenta = null; compositeInertia = null; unitTwists = null; coriolisBodyAcceleration = new SpatialAcceleration(getBodyFixedFrame(), input.getInertialFrame(), getBodyFixedFrame()); } else { parent.children.add(this); int nDoFs = getNumberOfDoFs(); bodyInertia = new SpatialInertia(rigidBody.getInertia()); compositeInertia = new SpatialInertia(); unitMomenta = IntStream.range(0, nDoFs).mapToObj(i -> new Momentum()).toArray(Momentum[]::new); unitTwists = new Twist[nDoFs]; for (int i = 0; i < nDoFs; i++) { unitTwists[i] = new Twist(getJoint().getUnitTwists().get(i)); unitTwists[i].changeFrame(getFrameAfterJoint()); } coriolisBodyAcceleration = new SpatialAcceleration(); } }
/** * Sums the inertia of all the rigid-bodies composing the subtree that originates at * {@code rootBody} including {@code rootBody}. * <p> * WARNING: This method generates garbage. * </p> * * @param rootBody the root of the subtree. * @return the subtree total inertia. */ public static SpatialInertia computeSubtreeInertia(RigidBodyReadOnly rootBody) { MovingReferenceFrame bodyFixedFrame = rootBody.getBodyFixedFrame(); SpatialInertia subtreeInertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); SpatialInertia bodyInertia = new SpatialInertia(); for (RigidBodyReadOnly subtreeBody : rootBody.subtreeList()) { bodyInertia.setIncludingFrame(subtreeBody.getInertia()); bodyInertia.changeFrame(bodyFixedFrame); subtreeInertia.add(bodyInertia); } return subtreeInertia; }
private void updateCenterOfMass() { if (isCenterOfMassUpToDate) return; centerOfMass.setToZero(referenceFrame); totalMass = 0.0; for (RigidBodyReadOnly rigidBody : rigidBodies) { SpatialInertiaReadOnly inertia = rigidBody.getInertia(); tempPoint.setIncludingFrame(inertia.getCenterOfMassOffset()); double mass = inertia.getMass(); tempPoint.changeFrame(referenceFrame); tempPoint.scale(mass); centerOfMass.add(tempPoint); totalMass += mass; } centerOfMass.scale(1.0 / totalMass); isCenterOfMassUpToDate = true; }
SpatialInertiaReadOnly inertia = rigidBody.getInertia();
public RecursionStep(RigidBodyReadOnly rigidBody, RecursionStep parent, int[] jointIndices) { this.rigidBody = rigidBody; this.parent = parent; this.jointIndices = jointIndices; if (isRoot()) { bodyInertia = null; coriolisBodyAcceleration = new SpatialAcceleration(getBodyFixedFrame(), input.getInertialFrame(), getBodyFixedFrame()); centroidalMomentumMatrixBlock = null; matrixFrameToBodyFixedFrameTransform = null; } else { parent.children.add(this); bodyInertia = new SpatialInertia(rigidBody.getInertia()); coriolisBodyAcceleration = new SpatialAcceleration(); centroidalMomentumMatrixBlock = new DenseMatrix64F(6, getJoint().getDegreesOfFreedom()); matrixFrameToBodyFixedFrameTransform = new RigidBodyTransform(); } }
SpatialInertiaReadOnly originalInertia = original.getInertia();
int nDoFs = getJoint().getDegreesOfFreedom(); bodyInertia = new SpatialInertia(rigidBody.getInertia()); jointWrench = new Wrench(); externalWrench = new Wrench(getBodyFixedFrame(), getBodyFixedFrame());
int nDoFs = getJoint().getDegreesOfFreedom(); bodyInertia = new SpatialInertia(rigidBody.getInertia()); biasAcceleration = new SpatialAcceleration(); biasWrench = new Wrench();