How to use

InverseDynamicsJoint

in

us.ihmc.robotics.screwTheory

public ReferenceFrame getFrameAfterParentJoint()
{
 return ankle.getFrameAfterJoint();
}

private void addJoint(CollisionBoxProvider collissionBoxProvider, InverseDynamicsJoint joint)
{
 List<CollisionShape> collisionMesh = collissionBoxProvider.getCollisionMesh(joint.getName());
 if (collisionMesh != null)
 {
   trackingCollisionShapes.add(new TrackingCollisionShape(joint.getFrameAfterJoint(), collisionMesh));
 }
 else
 {
   System.err.println(joint + " does not have a collission mesh");
 }
}

/**
* Returns the end-effector {@code RigidBody} of this Jacobian.
* The end-effector is the successor of the last joint this Jacobian considers.
* 
* @return the end-effector of this jacobian.
*/
public RigidBody getEndEffector()
{
 return joints[joints.length - 1].getSuccessor();
}

public static void getJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositionsToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   joint.getConfigurationMatrix(jointPositionsToPack, rowStart);
   rowStart += joint.getDegreesOfFreedom();
   if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint)
    rowStart++; // Because of stupid quaternions
 }
}

public static void getJointVelocitiesMatrix(Iterable<? extends InverseDynamicsJoint> joints, DenseMatrix64F jointVelocitiesMatrixToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   int dof = joint.getDegreesOfFreedom();
   joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart);
   rowStart += dof;
 }
}

public static void setJointPositions(InverseDynamicsJoint[] joints, DenseMatrix64F jointPositions)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   joint.setConfiguration(jointPositions, rowStart);
   rowStart += joint.getDegreesOfFreedom();
   if (joint instanceof SixDoFJoint || joint instanceof SphericalJoint)
    rowStart++; // Because of stupid quaternions
 }
}

private void printJointInformation(InverseDynamicsJoint joint, StringBuffer buffer)
 String jointName = joint.getName();
 InverseDynamicsJoint parentJoint = joint.getPredecessor().getParentJoint();
 if (parentJoint == null)
   ReferenceFrame frameBeforeJoint = joint.getFrameBeforeJoint();
   ReferenceFrame frameAfterParentJoint = parentJoint.getFrameAfterJoint();
   RigidBodyTransform comOffsetTransform = frameBeforeJoint.getTransformToDesiredFrame(frameAfterParentJoint);
   comOffsetTransform.getTranslation(jointOffset);
 RigidBody linkRigidBody = joint.getSuccessor();
 printLinkInformation(linkRigidBody, buffer);

  private static final void areJointsTheSame(InverseDynamicsJoint originalJoint, InverseDynamicsJoint targetJoint)
  {
   if(!(originalJoint.getClass().equals(targetJoint.getClass()) && 
      originalJoint.getName().equals(targetJoint.getName())))
   {
     throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint);
   }
   
  }
}

private void storeJointState()
{
 ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations);
 ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities);
 for (InverseDynamicsJoint joint : jointsInOrder)
 {
   DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1);
   joint.getTauMatrix(tauMatrix);
   DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1);
   CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce);
   Wrench jointWrench = storedJointWrenches.get(joint);
   jointWrench.set(joint.getFrameAfterJoint(), spatialForce);
   jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame());
 }
}

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

if (currentBody == joint.getPredecessor())
 joint.getPredecessorTwist(jointTwist);
 currentBody = joint.getSuccessor();
else if (currentBody == joint.getSuccessor())
 joint.getSuccessorTwist(jointTwist);
 currentBody = joint.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);
}

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

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 computeDegreesOfFreedom(InverseDynamicsJoint[] jointList)
{
 int ret = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   ret += joint.getDegreesOfFreedom();
 }
 return ret;
}

public void compute()
{
 int column = 0;
 for (int i = 0; i < rigidBodyList.size(); i++)
 {
   comScaledByMassMapIsUpdated.get(rigidBodyList.get(i)).setValue(false);
 }
 
 for (InverseDynamicsJoint joint : joints)
 {
   RigidBody childBody = joint.getSuccessor();
      FramePoint comPositionScaledByMass = getCoMScaledByMass(childBody);
   double subTreeMass = getSubTreeMass(childBody);
   GeometricJacobian motionSubspace = joint.getMotionSubspace();
   final List<Twist> allTwists = motionSubspace.getAllUnitTwists();
   for(int i = 0; i <  allTwists.size(); i++)
   {
    tempUnitTwist.set(allTwists.get(i));
    tempUnitTwist.changeFrame(rootFrame);
    setColumn(tempUnitTwist, comPositionScaledByMass, subTreeMass, column);
    column++;
   }
 }
 
 CommonOps.scale(inverseTotalMass, jacobianMatrix);
}

private void computeJointWrenchesAndTorques()
{
 for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--)
 {
   InverseDynamicsJoint joint = allJoints.get(jointIndex);
   RigidBody successor = joint.getSuccessor();
   Wrench jointWrench = jointWrenches.get(joint);
   jointWrench.set(netWrenches.get(successor));
   Wrench externalWrench = externalWrenches.get(successor);
   jointWrench.sub(externalWrench);
   List<InverseDynamicsJoint> childrenJoints = successor.getChildrenJoints();
   for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++)
   {
    InverseDynamicsJoint child = childrenJoints.get(childIndex);
    if (!jointsToIgnore.contains(child))
    {
      Wrench wrenchExertedOnChild = jointWrenches.get(child);
      ReferenceFrame successorFrame = successor.getBodyFixedFrame();
      wrenchExertedByChild.set(wrenchExertedOnChild);
      wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame);
      wrenchExertedByChild.scale(-1.0); // Action = -reaction
      wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame());
      jointWrench.sub(wrenchExertedByChild);
    }
   }
   joint.setTorqueFromWrench(jointWrench);
 }
}

private void setDesiredAccelerationsToZero()
{
 for (InverseDynamicsJoint joint : jointsInOrder)
 {
   joint.setDesiredAccelerationToZero();
   joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0);
 }
}

public static void setVelocities(InverseDynamicsJoint[] jointList, DenseMatrix64F jointVelocities)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   joint.setVelocity(jointVelocities, rowStart);
   rowStart += joint.getDegreesOfFreedom();
 }
}

public static void getDesiredJointAccelerationsMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F desiredJointAccelerationsMatrixToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   int dof = joint.getDegreesOfFreedom();
   joint.getDesiredAccelerationMatrix(desiredJointAccelerationsMatrixToPack, rowStart);
   rowStart += dof;
 }
}