How to use

getPredecessor

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

/**
* Returns the base {@code RigidBody} of this Jacobian.
* The base is the predecessor of the first joint that this Jacobian considers.
* 
* @return the base of this Jacobian.
*/
public RigidBody getBase()
{
 return joints[0].getPredecessor();
}

private void computeSuccessorAcceleration(InverseDynamicsJoint joint)
{
 RigidBody predecessor = joint.getPredecessor();
 RigidBody successor = joint.getSuccessor();
 ReferenceFrame successorFrame = successor.getBodyFixedFrame();
 joint.getPredecessorTwist(tempJointTwist);
 if (!doVelocityTerms)
   tempJointTwist.setToZero();
 if (useDesireds)
   joint.getDesiredSuccessorAcceleration(tempJointAcceleration);
 else
   joint.getSuccessorAcceleration(tempJointAcceleration);
 if (!doAccelerationTerms)
   tempJointAcceleration.setToZero();
 twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor);
 if (!doVelocityTerms)
   tempTwistFromWorld.setToZero();
 SpatialAccelerationVector successorAcceleration = accelerations.get(successor);
 successorAcceleration.set(accelerations.get(predecessor));
 successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld);
 successorAcceleration.add(tempJointAcceleration);
}

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

public void computeJacobianDerivativeTerm(SpatialAccelerationVector accelerationToPack)
{
 ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame();
 twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame);
 accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame);
 RigidBody currentBody = jacobian.getEndEffector();
 for (int i = jacobian.getJointsInOrder().length - 1; i >= 0; i--)
 {
   twistOfCurrentWithRespectToBase.changeFrame(currentBody.getBodyFixedFrame());
   InverseDynamicsJoint joint = jacobian.getJointsInOrder()[i];
   if (currentBody == joint.getPredecessor())
   {
    joint.getPredecessorTwist(jointTwist);
    currentBody = joint.getSuccessor();
   }
   else
   {
    joint.getSuccessorTwist(jointTwist);
    currentBody = joint.getPredecessor();            
   }
   zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame());
   zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist);
   zeroAcceleration.add(accelerationToPack);
   accelerationToPack.set(zeroAcceleration);
   jointTwist.invert();
   twistOfCurrentWithRespectToBase.add(jointTwist);
 }
 accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame);
}

private RigidBody addAllPrecedingJoints(RigidBody body)
{
 ArrayList<InverseDynamicsJoint> jointsTillRootBody = new ArrayList<InverseDynamicsJoint>();
 RigidBody currentBody = body;
 while (currentBody.getParentJoint() != null)
 {
   jointsTillRootBody.add(currentBody.getParentJoint());
   currentBody = currentBody.getParentJoint().getPredecessor();
 }
 for (int i = jointsTillRootBody.size() - 1; i >= 0; i--)
 {
   allJoints.add(jointsTillRootBody.get(i));
 }
 return currentBody;
}

if (currentBody == joint.getPredecessor())
 currentBody = joint.getPredecessor();

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 RigidBody getRootBody(RigidBody body)
{
 RigidBody ret = body;
 while (ret.getParentJoint() != null)
 {
   ret = ret.getParentJoint().getPredecessor();
 }
 return ret;
}

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 RigidBody[] computeSubtreeSuccessors(InverseDynamicsJoint... joints)
{
 ArrayList<RigidBody> rigidBodySuccessors = new ArrayList<RigidBody>();
 ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>();
 for (InverseDynamicsJoint joint : joints)
 {
   rigidBodyStack.add(joint.getPredecessor());
 }
 while (!rigidBodyStack.isEmpty())
 {
   RigidBody currentBody = rigidBodyStack.remove(0);
   List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints();
   for (InverseDynamicsJoint joint : childrenJoints)
   {
    rigidBodyStack.add(joint.getSuccessor());
    rigidBodySuccessors.add(joint.getSuccessor());
   }
 }
 RigidBody[] ret = new RigidBody[rigidBodySuccessors.size()];
 return rigidBodySuccessors.toArray(ret);
}

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

public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) 
{
 registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName());
 this.forceSensorData = forceSensorData;
 //ground reaction wrench on joints
 yoTorqueInJoints = new ArrayList<>();
 RigidBody currentBody = sensorLinkBody;
 for(int i=0;i<numberOfJointFromSensor;i++)
 {
   FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis();
   yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry)));
   currentBody=currentBody.getParentJoint().getPredecessor();
 }
}
@Override

public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) 
{
 registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName());
 this.forceSensorData = forceSensorData;
 //ground reaction wrench on joints
 yoTorqueInJoints = new ArrayList<>();
 RigidBody currentBody = sensorLinkBody;
 for(int i=0;i<numberOfJointFromSensor;i++)
 {
   FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis();
   yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry)));
   currentBody=currentBody.getParentJoint().getPredecessor();
 }
}
@Override

public static InverseDynamicsJoint[] createJointPath(RigidBody start, RigidBody end)
{
 boolean flip = false;
 RigidBody descendant = start;
 RigidBody ancestor = end;
 int pathLength = computeDistanceToAncestor(descendant, ancestor);
 if (pathLength < 0)
 {
   flip = true;
   descendant = end;
   ancestor = start;
   pathLength = computeDistanceToAncestor(end, start);
 }
 InverseDynamicsJoint[] ret = new InverseDynamicsJoint[pathLength];
 RigidBody currentBody = descendant;
 int i = 0;
 while (currentBody != ancestor)
 {
   int j = flip ? pathLength - 1 - i : i;
   InverseDynamicsJoint parentJoint = currentBody.getParentJoint();
   ret[j] = parentJoint;
   currentBody = parentJoint.getPredecessor();
   i++;
 }
 return ret;
}

InverseDynamicsJoint parentJoint = currentBody.getParentJoint();
jointPathToPack[j] = parentJoint;
currentBody = parentJoint.getPredecessor();
i++;

InverseDynamicsJoint parentJoint = joint.getPredecessor().getParentJoint();
if (parentJoint == null)

RigidBody predecessor = parentJoint.getPredecessor();
Twist twistOfPredecessor = computeOrGetTwistOfBody(predecessor);
twist = assignAndGetEmptyTwist(rigidBody);