/** * Third and last pass that can be done iteratively and each iteration is independent. * <p> * Computes the Jacobian block corresponding to this joint. * </p> * * @param inverseOfTotalMass the inverse of the total system mass. */ public void passThree(double inverseOfTotalMass) { if (isRoot()) return; // Compute the Jacobian matrix block corresponding to the current joint. JointReadOnly joint = getJoint(); for (int i = 0; i < joint.getDegreesOfFreedom(); i++) { jointUnitTwist.setIncludingFrame(joint.getUnitTwists().get(i)); jointUnitTwist.changeFrame(jacobianFrame); jacobianColumn.cross(jointUnitTwist.getAngularPart(), centerOfMassTimesMass); jacobianColumn.scaleAdd(subTreeMass, jointUnitTwist.getLinearPart(), jacobianColumn); jacobianColumn.get(0, i, jacobianJointBlock); } CommonOps.scale(inverseOfTotalMass, jacobianJointBlock); for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { int column = jointIndices[dofIndex]; CommonOps.extract(jacobianJointBlock, 0, 3, dofIndex, dofIndex + 1, jacobianMatrix, 0, column); } }
/** * 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()); } }