/** * Assuming {@code matrix} represents a proper 6-by-6 inertia matrix, it is added to this * inertia. * * @param matrix the inertia matrix to add. Not modified. */ public void add(DenseMatrix64F matrix) { MecanoTools.addEquals(0, 0, matrix, angularInertia); MecanoTools.addEquals(3, 3, matrix, linearInertia); MecanoTools.addEquals(0, 3, matrix, crossInertia); }
/** {@inheritDoc} */ @Override default void setJointConfiguration(JointReadOnly other) { MecanoTools.checkTypeAndCast(other, FixedJointReadOnly.class); }
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);
/** * Calculates the cross product of {@code tuple1} and {@code tuple2} and adds the result to this * vector's linear part. * <p> * {@code this.linearPart = this.linearPart + tuple1 x tuple2}. * </p> * * @param tuple1 the first tuple in the cross product. Not modified. * @param tuple2 the second tuple in the cross product. Not modified. */ default void addCrossToLinearPart(Tuple3DReadOnly tuple1, Tuple3DReadOnly tuple2) { MecanoTools.addCrossToVector(tuple1, tuple2, getLinearPart()); }
/** * Creates the reference frame that is at the joint and rigidly attached to the joint's predecessor. * * @param joint the joint to which the new frame is for. Not modified. * @param transformToParent the transform from the new frame to the frame after the parent joint. * Not modified. * @return the new frame before joint. */ public static MovingReferenceFrame newFrameBeforeJoint(JointReadOnly joint, RigidBodyTransform transformToParent) { String beforeJointName = "before" + MecanoTools.capitalize(joint.getName()); return newJointFrame(joint, transformToParent, beforeJointName); }
/** * 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()); }
symmetricPositiveDefiniteMatrix3D.multiplyTransposeOther(lowerTriangular); MecanoTools.checkIfMatrix3DIsSymmetric(symmetricPositiveDefiniteMatrix3D, 1.0e-8); checkEigenValuesRealAndPositive(symmetricPositiveDefiniteMatrix3D, 1e-10);
/** * Calculates the cross product of {@code tuple1} and {@code tuple2} and adds the result to this * vector's angular part. * <p> * {@code this.angularPart = this.angularPart + tuple1 x tuple2}. * </p> * * @param tuple1 the first tuple in the cross product. Not modified. * @param tuple2 the second tuple in the cross product. Not modified. */ default void addCrossToAngularPart(Tuple3DReadOnly tuple1, Tuple3DReadOnly tuple2) { MecanoTools.addCrossToVector(tuple1, tuple2, getAngularPart()); }
/** * Creates the reference frame that is at the joint and rigidly attached to the joint's successor. * * @param joint the joint to which the new frame is for. Not modified. * @return the new frame after joint. */ public static MovingReferenceFrame newFrameAfterJoint(JointReadOnly joint) { MovingReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint(); if (frameBeforeJoint == null) throw new NullPointerException("The frameBeforeJoint has to be created before the frameAfterJoint."); return new MovingReferenceFrame("after" + MecanoTools.capitalize(joint.getName()), frameBeforeJoint) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { joint.getJointConfiguration(transformToParent); } @Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { twistRelativeToParentToPack.setIncludingFrame(joint.getJointTwist()); } }; }
/** {@inheritDoc} */ @Override default void setJointWrench(JointReadOnly other) { MecanoTools.checkTypeAndCast(other, FixedJointReadOnly.class); }
MecanoTools.addCrossToVector(bodyTwist.getAngularPart(), bodyTwist.getLinearPart(), linearAccelerationToPack);
/** {@inheritDoc} */ @Override default void setJointTwist(JointReadOnly other) { MecanoTools.checkTypeAndCast(other, FixedJointReadOnly.class); }
addCrossToVector(centerOfMassOffset, angularAcceleration, dynamicForceToPack);
/** {@inheritDoc} */ @Override default void setJointAcceleration(JointReadOnly other) { MecanoTools.checkTypeAndCast(other, FixedJointReadOnly.class); }
MecanoTools.addCrossToVector(bodyTwist.getAngularPart(), bodyFixedPoint, linearAccelerationToPack); // (w x p) + v MecanoTools.addCrossToVector(getAngularPart(), bodyFixedPoint, linearAccelerationToPack); // (wDot x p) + w x ((w x p) + v)
/** {@inheritDoc} */ @Override default void setJointWrench(JointReadOnly other) { setJointWrench(MecanoTools.checkTypeAndCast(other, SphericalJointReadOnly.class)); }
/** {@inheritDoc} */ @Override default void setJointConfiguration(JointReadOnly other) { setJointConfiguration(MecanoTools.checkTypeAndCast(other, PlanarJointReadOnly.class)); }
/** {@inheritDoc} */ @Override default void setJointAcceleration(JointReadOnly other) { setJointAcceleration(MecanoTools.checkTypeAndCast(other, PlanarJointReadOnly.class)); }
/** {@inheritDoc} */ @Override default void setJointConfiguration(JointReadOnly other) { setJointConfiguration(MecanoTools.checkTypeAndCast(other, SixDoFJointReadOnly.class)); }
/** {@inheritDoc} */ @Override default void setJointTwist(JointReadOnly other) { setJointTwist(MecanoTools.checkTypeAndCast(other, SixDoFJointReadOnly.class)); }