public static int[] createParentMap(RigidBody[] allRigidBodiesInOrder) { int[] parentMap = new int[allRigidBodiesInOrder.length]; List<RigidBody> rigidBodiesInOrderList = Arrays.asList(allRigidBodiesInOrder); // this doesn't have to be fast for (int i = 0; i < allRigidBodiesInOrder.length; i++) { RigidBody currentBody = allRigidBodiesInOrder[i]; if (currentBody.isRootBody()) { parentMap[i] = -1; } else { RigidBody parentBody = currentBody.getParentJoint().getPredecessor(); parentMap[i] = rigidBodiesInOrderList.indexOf(parentBody); } } return parentMap; }
public static int computeDistanceToAncestor(RigidBody descendant, RigidBody ancestor) { int ret = 0; RigidBody currentBody = descendant; while (!currentBody.isRootBody() && (currentBody != ancestor)) { ret++; currentBody = currentBody.getParentJoint().getPredecessor(); } if (currentBody != ancestor) ret = -1; return ret; }
public static boolean isAncestor(RigidBody candidateDescendant, RigidBody ancestor) { RigidBody currentBody = candidateDescendant; while (!currentBody.isRootBody()) { if (currentBody == ancestor) { return true; } currentBody = currentBody.getParentJoint().getPredecessor(); } return currentBody == ancestor; }
public static void computeRigidBodiesFromRootToThisJoint(ArrayList<RigidBody> rigidBodySuccessorsToPack, InverseDynamicsJoint joint) { RigidBody predecessorBody = joint.getPredecessor(); if (predecessorBody == null) return; if (predecessorBody.isRootBody()) return; rigidBodySuccessorsToPack.add(predecessorBody); InverseDynamicsJoint parentJoint = predecessorBody.getParentJoint(); if (parentJoint == null) return; computeRigidBodiesFromRootToThisJoint(rigidBodySuccessorsToPack, parentJoint); }
if (!currentBody.isRootBody())
if (!currentBody.isRootBody())
if (predecessorOriginal.isRootBody())
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
public static PrismaticJoint addPrismaticJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; PrismaticJoint joint = new PrismaticJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; RevoluteJoint joint = new RevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }