/** * 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(); }
/** * 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; } }
if (!getCenterOfMassOffset().epsilonEquals(other.getCenterOfMassOffset(), epsilon)) return false; if (!getMomentOfInertia().epsilonEquals(other.getMomentOfInertia(), epsilon)) return false; return true;
/** * Sets this spatial inertia to {@code other}. * * @param other the other spatial inertia to copy values from. Not modified. * @throws ReferenceFrameMismatchException if {@code other} does not have the same frames as * {@code this}. */ default void set(SpatialInertiaReadOnly other) { checkReferenceFrameMatch(other); getMomentOfInertia().set(other.getMomentOfInertia()); setMass(other.getMass()); getCenterOfMassOffset().set(other.getCenterOfMassOffset()); }
/** * Sets this spatial inertia to {@code other}. * * @param other the other spatial inertia to copy values and reference frames from. Not modified. */ default void setIncludingFrame(SpatialInertiaReadOnly other) { setBodyFrame(other.getBodyFrame()); setReferenceFrame(other.getReferenceFrame()); getMomentOfInertia().set(other.getMomentOfInertia()); setMass(other.getMass()); getCenterOfMassOffset().set(other.getCenterOfMassOffset()); }
/** * Calculates the kinetic co-energy as introduced in the Ph.D. thesis of Vincent Duindam entitled * <i>"Port-Based Modeling and Control for Efficient Bipedal Walking Robots"</i>, page 38. * <p> * This method does not require this spatial inertia to be expressed in any particular frame. * </p> * * @param twist the twist of {@code this.bodyFrame} with respect to world and expressed in * {@code this.expressedInFrame}. Not modified. * @return the value of the kinetic co-energy. * @throws ReferenceFrameMismatchException if * {@code twist.getBodyFrame() != this.getBodyFrame()}, or if the given {@code twist} * is not expressed in the same reference frame as {@code this}. * @throws RuntimeException if the base frame of the given {@code twist} is not an inertial * frame. */ default double computeKineticCoEnergy(TwistReadOnly twist) { twist.checkBodyFrameMatch(getBodyFrame()); twist.getBaseFrame().checkIsAStationaryFrame(); twist.checkExpressedInFrameMatch(getReferenceFrame()); return MecanoTools.computeKineticCoEnergy(getMomentOfInertia(), getMass(), getCenterOfMassOffset(), twist.getAngularPart(), twist.getLinearPart()); }
/** * Sets this inertia to be equivalent to the given {@code spatialInertiaReadOnly}. * * @param spatialInertiaReadOnly the spatial inertia to copy. Not modified. */ public void setIncludingFrame(SpatialInertiaReadOnly spatialInertiaReadOnly) { expressedInFrame = spatialInertiaReadOnly.getReferenceFrame(); angularInertia.set(spatialInertiaReadOnly.getMomentOfInertia()); linearInertia.setToZero(); linearInertia.setM00(spatialInertiaReadOnly.getMass()); linearInertia.setM11(spatialInertiaReadOnly.getMass()); linearInertia.setM22(spatialInertiaReadOnly.getMass()); crossInertia.setToTildeForm(spatialInertiaReadOnly.getCenterOfMassOffset()); crossInertia.scale(spatialInertiaReadOnly.getMass()); }
Matrix3DReadOnly momentOfInertia = originalInertia.getMomentOfInertia(); RigidBodyTransform inertiaPose = new RigidBodyTransform(original.getBodyFixedFrame().getTransformToParent()); RigidBodyBasics clone = rigidBodyBuilder.build(nameOriginal + cloneSuffix, parentJointOfClone, momentOfInertia, mass, inertiaPose);
/** * 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()); } }
getMomentOfInertia().get(startRow, startColumn, matrixToPack);
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,
twist.checkExpressedInFrameMatch(getReferenceFrame()); spatialInertia.getMomentOfInertia().transform(twist.getAngularPart(), getAngularPart()); getLinearPart().set(twist.getLinearPart());