/** * Checks that the reference frames used by {@code this} correspond to the given ones. * * @param bodyFrame the query for the body frame. * @param expressedInFrame the query for the "expressed in frame". * @throws ReferenceFrameMismatchException if the reference frames are not the same: * {@code this.getBodyFrame() != bodyFrame} or * {@code this.getReferenceFrame() != expressedInFrame}. */ default void checkReferenceFrameMatch(ReferenceFrame bodyFrame, ReferenceFrame expressedInFrame) { ReferenceFrameHolder.super.checkReferenceFrameMatch(expressedInFrame); checkBodyFrameMatch(bodyFrame); }
checkIfCenterOfMassOffsetIsZero(); // otherwise this operation would be a lot less efficient computeDynamicWrench(acceleration, twist, dynamicWrenchToPack);
/** * Checks that all frames, i.e. the body frame and the "expressed in frame", are identical * between the two spatial inertia matrices. * * @param other the other object holding onto the reference frames to compare against the * reference frames held by {@code this}. Not modified. * @throws ReferenceFrameMismatchException if the reference frames are not the same: * {@code this.getBodyFrame() != other.getBodyFrame()} or * {@code this.getReferenceFrame() != other.getReferenceFrame()}. */ default void checkReferenceFrameMatch(SpatialInertiaReadOnly other) throws ReferenceFrameMismatchException { ReferenceFrame expressedInFrame = other.getReferenceFrame(); ReferenceFrame bodyFrame = other.getBodyFrame(); checkReferenceFrameMatch(bodyFrame, expressedInFrame); }
/** * Tests if {@code this} and {@code other} represent the same spatial inertia to an * {@code epsilon}. * <p> * It is likely that the implementation of this method will change in the future as the * definition of "geometrically-equal" for spatial inertia might evolve. In the meantime, the * current assumption is that two spatial inertia matrices are geometrically equal if they are * epsilon equal, see {@link #epsilonEquals(SpatialInertiaReadOnly, double)}. * </p> * * @param other the other spatial inertia to compare against this. Not modified. * @param epsilon the tolerance to use for the comparison. * @return {@code true} if the two spatial inertia matrices represent the same physical quantity, * {@code false} otherwise. * @throws ReferenceFrameMismatchException if the reference frames of {@code other} do not * respectively match the reference frames of {@code this}. */ default boolean geometricallyEquals(SpatialInertiaReadOnly other, double epsilon) { checkReferenceFrameMatch(other); return epsilonEquals(other, epsilon); }
/** * Tests on a per component basis, if this spatial inertia matrix is exactly equal to * {@code other} and have the same reference frames. * * @param other the other spatial inertia to compare against this. Not modified. * @return {@code true} if the two spatial inertia matrices are exactly equal component-wise, * {@code false} otherwise. */ default boolean equals(SpatialInertiaReadOnly other) { if (other == null) return false; if (getBodyFrame() != other.getBodyFrame()) return false; if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getMomentOfInertia().equals(other.getMomentOfInertia())) return false; if (getMass() != other.getMass()) return false; if (!getCenterOfMassOffset().equals(other.getCenterOfMassOffset())) return false; return true; } }
checkBodyFrameMatch(getReferenceFrame()); twist.checkBodyFrameMatch(getBodyFrame()); twist.getBaseFrame().checkIsAStationaryFrame(); twist.checkExpressedInFrameMatch(getReferenceFrame()); angularVelocity = twist.getAngularPart(); linearVelocity = twist.getLinearPart(); acceleration.checkBodyFrameMatch(getBodyFrame()); acceleration.getBaseFrame().checkIsAStationaryFrame(); acceleration.checkExpressedInFrameMatch(getReferenceFrame()); dynamicWrenchToPack.setToZero(getBodyFrame(), getReferenceFrame()); if (isCenterOfMassOffsetZero()) MecanoTools.computeDynamicMomentFast(getMomentOfInertia(), angularAcceleration, angularVelocity, dynamicMoment); MecanoTools.computeDynamicForceFast(getMass(), linearAcceleration, angularVelocity, linearVelocity, dynamicForce); MecanoTools.computeDynamicMoment(getMomentOfInertia(), getMass(), getCenterOfMassOffset(), angularAcceleration, linearAcceleration, angularVelocity, linearVelocity, dynamicMoment); MecanoTools.computeDynamicForce(getMass(), getCenterOfMassOffset(), angularAcceleration, linearAcceleration, angularVelocity, linearVelocity, dynamicForce);
/** * Adds the other spatial inertia to this. * * @param other the other inertia to add. Not modified. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ default void add(SpatialInertiaReadOnly other) { other.checkReferenceFrameMatch(getReferenceFrame()); getMomentOfInertia().add(other.getMomentOfInertia()); getCenterOfMassOffset().scale(getMass()); getCenterOfMassOffset().scaleAdd(other.getMass(), other.getCenterOfMassOffset(), getCenterOfMassOffset()); setMass(getMass() + other.getMass()); if (Math.abs(getMass()) >= 1.0e-7) getCenterOfMassOffset().scale(1.0 / getMass()); } }
/** * Tests whether any component of this spatial inertia matrix is equal to {@link Double#NaN}. * * @return {@code true} if at least one component of this inertia is {@link Double#NaN}, * {@code false} otherwise. */ default boolean containsNaN() { return getMomentOfInertia().containsNaN() || Double.isNaN(getMass()) || getCenterOfMassOffset().containsNaN(); }
spatialInertia.get(inertiaMatrix); return "Spatial inertia of " + spatialInertia.getBodyFrame() + " expressed in " + spatialInertia.getReferenceFrame() + ":\n" + getDenseMatrix64FString(format, inertiaMatrix);
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; }
/** * Tests if the center of mass offset is negligible. * * @return {@code true} if the center of mass offset can be ignored, {@code false} otherwise. */ default boolean isCenterOfMassOffsetZero() { return getCenterOfMassOffset().lengthSquared() < COM_OFFSET_ZERO_EPSILON; }
/** * 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; }
/** {@inheritDoc} */ @Override default boolean containsNaN() { return SpatialInertiaReadOnly.super.containsNaN(); }
/** * Packs this spatial inertia matrix into the given {@code DenseMatrix64F} as follows: * * <pre> * / J<sub>x,x</sub> J<sub>x,y</sub> J<sub>x,z</sub> 0 -mz my \ * | J<sub>x,y</sub> J<sub>y,y</sub> J<sub>y,z</sub> mz 0 -mx | * I = | J<sub>x,z</sub> J<sub>y,z</sub> J<sub>z,z</sub> -my mx 0 | * | 0 mz -my m 0 0 | * | -mz 0 mx 0 m 0 | * \ my -mx 0 0 0 m / * </pre> * * @param matrixToPack the matrix in which this spatial inertia is stored. Modified. */ default void get(DenseMatrix64F matrixToPack) { get(0, 0, matrixToPack); }
/** * Checks if the body frame held by {@code this} matches the query {@code bodyFrame}. * * @param bodyFrame the query to compare against the body frame held by {@code this}. Not * modified. * @throws ReferenceFrameMismatchException if the two reference frames are not the same: * {@code this.getBodyFrame() != bodyFrame}. */ default void checkBodyFrameMatch(ReferenceFrame bodyFrame) { if (getBodyFrame() != bodyFrame) throw new ReferenceFrameMismatchException("bodyFrame mismatch: this.bodyFrame = " + getBodyFrame() + ", other bodyFrame = " + bodyFrame); }
/** * Asserts on a per component basis that the two spatial inertia matrices are equal to an * {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the automated message. * @param expected the expected spatial inertia matrix. Not modified. * @param actual the actual spatial inertia matrix. Not modified. * @param epsilon the tolerance to use. * @param format the format to use for printing each component when an {@code AssertionError} is * thrown. * @throws AssertionError if the two spatial inertia matrices are not equal. If only one of the * arguments is equal to {@code null}. */ public static void assertSpatialInertiaEquals(String messagePrefix, SpatialInertiaReadOnly expected, SpatialInertiaReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
public void computeCentroidalConvectiveTerm() { if (!isRoot()) { SpatialInertiaReadOnly inertia = bodyInertia; coriolisBodyAcceleration.setIncludingFrame(parent.coriolisBodyAcceleration); getJoint().getPredecessorTwist(intermediateTwist); coriolisBodyAcceleration.changeFrame(getBodyFixedFrame(), intermediateTwist, parent.getBodyFixedFrame().getTwistOfFrame()); coriolisBodyAcceleration.setBodyFrame(getBodyFixedFrame()); inertia.computeDynamicWrench(coriolisBodyAcceleration, getBodyFixedFrame().getTwistOfFrame(), netCoriolisBodyWrench); netCoriolisBodyWrench.changeFrame(centroidalMomentumFrame); centroidalConvectiveTerm.add(netCoriolisBodyWrench); } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).computeCentroidalConvectiveTerm(); }
if (getBodyFrame() != other.getBodyFrame()) return false; if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!EuclidCoreTools.epsilonEquals(getMass(), other.getMass(), epsilon)) return false; if (!getCenterOfMassOffset().epsilonEquals(other.getCenterOfMassOffset(), epsilon)) return false; if (!getMomentOfInertia().epsilonEquals(other.getMomentOfInertia(), epsilon)) return false; return true;
spatialInertia.checkReferenceFrameMatch(twist.getReferenceFrame()); twist.checkExpressedInFrameMatch(getReferenceFrame()); spatialInertia.getMomentOfInertia().transform(twist.getAngularPart(), getAngularPart()); getLinearPart().set(twist.getLinearPart()); if (!spatialInertia.isCenterOfMassOffsetZero()) double z = getAngularPartZ(); getAngularPart().cross(spatialInertia.getCenterOfMassOffset(), twist.getLinearPart()); getAngularPart().scale(spatialInertia.getMass()); getAngularPart().add(x, y, z); addCrossToLinearPart(twist.getAngularPart(), spatialInertia.getCenterOfMassOffset()); getLinearPart().scale(spatialInertia.getMass());
getMomentOfInertia().get(startRow, startColumn, matrixToPack); MecanoTools.toTildeForm(getMass(), getCenterOfMassOffset(), false, startRow, startColumn + 3, matrixToPack); MecanoTools.toTildeForm(getMass(), getCenterOfMassOffset(), true, startRow + 3, startColumn, matrixToPack); matrixToPack.set(startRow + i, startColumn + i, getMass()); matrixToPack.set(startRow + i, startColumn + (i + 1) % 3, 0.0); matrixToPack.set(startRow + i, startColumn + (i + 2) % 3, 0.0);