private double getSubTreeMass(RigidBody rigidBody) { if (!subTreeMassMap.containsKey(rigidBody)) subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); MutableDouble subTreeMass = subTreeMassMap.get(rigidBody); if (subTreeMass.doubleValue() > 0.0) { return subTreeMass.doubleValue(); } else { double curSubTreeMass = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { double childSubTreeMass = getSubTreeMass(childrenJoints.get(i).getSuccessor()); curSubTreeMass = curSubTreeMass + childSubTreeMass; } subTreeMass.setValue(curSubTreeMass); return curSubTreeMass; } }
public void compute() { int column = 0; for (int i = 0; i < rigidBodyList.size(); i++) { comScaledByMassMapIsUpdated.get(rigidBodyList.get(i)).setValue(false); } for (InverseDynamicsJoint joint : joints) { RigidBody childBody = joint.getSuccessor(); FramePoint comPositionScaledByMass = getCoMScaledByMass(childBody); double subTreeMass = getSubTreeMass(childBody); GeometricJacobian motionSubspace = joint.getMotionSubspace(); final List<Twist> allTwists = motionSubspace.getAllUnitTwists(); for(int i = 0; i < allTwists.size(); i++) { tempUnitTwist.set(allTwists.get(i)); tempUnitTwist.changeFrame(rootFrame); setColumn(tempUnitTwist, comPositionScaledByMass, subTreeMass, column); column++; } } CommonOps.scale(inverseTotalMass, jacobianMatrix); }