private static LinkedHashMap<InverseDynamicsJoint, List<Twist>> extractTwistsFromJoints(
      InverseDynamicsJoint[] joints) {
    checkJointOrder(joints);

    LinkedHashMap<InverseDynamicsJoint, List<Twist>> ret =
        new LinkedHashMap<InverseDynamicsJoint, List<Twist>>();
    for (int i = 0; i < joints.length; i++) {
      InverseDynamicsJoint joint = joints[i];
      ret.put(joint, joint.getMotionSubspace().unitTwistMap.get(joint));
    }

    return ret;
  }
  public void compute() {
    MatrixTools.setToZero(massMatrix);

    for (int i = 0; i < allRigidBodiesInOrder.length; i++) {
      crbInertiasInOrder[i].set(allRigidBodiesInOrder[i].getInertia());
    }

    for (int i = allRigidBodiesInOrder.length - 1; i >= 0; i--) {
      RigidBody currentBody = allRigidBodiesInOrder[i];
      InverseDynamicsJoint parentJoint = currentBody.getParentJoint();
      CompositeRigidBodyInertia currentBodyInertia = crbInertiasInOrder[i];
      GeometricJacobian motionSubspace = parentJoint.getMotionSubspace();

      setUnitMomenta(currentBodyInertia, motionSubspace);
      setDiagonalTerm(i, motionSubspace);
      setOffDiagonalTerms(i);
      buildCrbInertia(i);
    }
  }
 public GeometricJacobian(InverseDynamicsJoint joint, ReferenceFrame jacobianFrame) {
   this(joint, joint.getMotionSubspace().getAllUnitTwists(), jacobianFrame);
 }