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);
}
