/** * Updates {@link #bodyPathFromBaseToEndEffector} based on the new {@code base} and * {@code endEffector}. */ private void updateRigidBodyPath() { bodyPathFromBaseToEndEffector.clear(); int pathLength = MultiBodySystemTools.computeDistanceToAncestor(endEffector, base) + 1; while (bodyPathFromBaseToEndEffector.size() < pathLength) bodyPathFromBaseToEndEffector.add(null); int index = pathLength - 1; RigidBodyReadOnly currentBody = endEffector; while (currentBody != base) { bodyPathFromBaseToEndEffector.set(index--, currentBody); currentBody = currentBody.getParentJoint().getPredecessor(); } bodyPathFromBaseToEndEffector.set(0, base); }
RigidBodyBasics descendant = start; RigidBodyBasics ancestor = end; int pathLength = MultiBodySystemTools.computeDistanceToAncestor(descendant, ancestor); if (pathLength < 0) descendant = end; ancestor = start; pathLength = MultiBodySystemTools.computeDistanceToAncestor(end, start);
RigidBodyReadOnly descendant = start; RigidBodyReadOnly ancestor = end; int pathLength = computeDistanceToAncestor(descendant, ancestor); descendant = end; ancestor = start; pathLength = computeDistanceToAncestor(end, start);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDistanceToAncestor() { int numberOfJoints = joints.size(), numberOfBodies = numberOfJoints + 1; RigidBodyBasics[] allBodies = new RigidBodyBasics[numberOfBodies]; allBodies[0] = elevator; for(int i = 0; i < numberOfJoints; i++) { allBodies[i+1] = joints.get(i).getSuccessor(); } RigidBodyBasics d0 = allBodies[0]; //elevator RigidBodyBasics d1 = allBodies[1]; //chainAbody0 RigidBodyBasics d2 = allBodies[2]; //chainAbody1 RigidBodyBasics d3 = allBodies[3]; //chainAbody2 assertEquals(0, MultiBodySystemTools.computeDistanceToAncestor(d0, d0)); //self assertEquals(3, MultiBodySystemTools.computeDistanceToAncestor(d3, d0)); //ancestor assertEquals(-1, MultiBodySystemTools.computeDistanceToAncestor(d0, d3)); //descendant }
/** * Finds amongst the given {@code joints} which is the closest to the root body. * * @param joints the list of joints to search. Not modified. * @return the closest joint to the root body. */ public static JointReadOnly getClosestJointToRoot(List<? extends JointReadOnly> joints) { JointReadOnly closest = joints.get(0); RigidBodyReadOnly rootBody = MultiBodySystemTools.getRootBody(closest.getPredecessor()); int distanceToRoot = MultiBodySystemTools.computeDistanceToAncestor(closest.getPredecessor(), rootBody); for (int i = 1; i < joints.size(); i++) { JointReadOnly candidate = joints.get(i); int candidateDistanceToRoot = MultiBodySystemTools.computeDistanceToAncestor(candidate.getPredecessor(), rootBody); if (candidateDistanceToRoot < distanceToRoot) { distanceToRoot = candidateDistanceToRoot; closest = candidate; } } return closest; } }
int distanceFromBase = MultiBodySystemTools.computeDistanceToAncestor(endEffector, base); if (distanceFromBase < 0) throw new IllegalArgumentException("The base: " + base.getName() + "is not an ancestor of the given end-effector: " + endEffector.getName());