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