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