// TODO: Use ScrewTools
private void populateMapsAndLists(
ArrayList<InverseDynamicsJoint> jointsInOrderListToPack,
ArrayList<RigidBody> rigidBodiesInOrderListToPack) {
ArrayList<RigidBody> morgue = new ArrayList<RigidBody>();
morgue.add(rootBody);
while (!morgue.isEmpty()) {
RigidBody currentBody = morgue.get(0);
if (!currentBody.isRootBody()) {
rigidBodiesInOrderListToPack.add(currentBody);
}
if (currentBody.hasChildrenJoints()) {
List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints();
for (InverseDynamicsJoint joint : childrenJoints) {
if (!jointsToIgnore.contains(joint)) {
RigidBody successor = joint.getSuccessor();
if (successor != null) {
if (rigidBodiesInOrderListToPack.contains(successor)) {
throw new RuntimeException("This algorithm doesn't do loops.");
}
jointsInOrderListToPack.add(joint);
morgue.add(successor);
}
}
}
}
morgue.remove(currentBody);
}
}
private static void checkJointOrder(InverseDynamicsJoint[] joints) {
for (int i = 1; i < joints.length; i++) {
InverseDynamicsJoint joint = joints[i];
InverseDynamicsJoint previousJoint = joints[i - 1];
if (ScrewTools.isAncestor(previousJoint.getPredecessor(), joint.getPredecessor()))
throw new RuntimeException("joints must be in order from ancestor to descendant");
}
}
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;
}
@Override
public String toString() {
StringBuilder builder = new StringBuilder();
builder.append("Jacobian. jacobianFrame = " + jacobianFrame + ". Joints:\n");
for (InverseDynamicsJoint joint : unitTwistMap.keySet()) {
builder.append(joint.getClass().getSimpleName() + " " + joint.getName() + "\n");
}
builder.append("\n");
builder.append(jacobian.toString());
return builder.toString();
}
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);
}
