How to use

getMotionSubspace

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

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

/**
* Creates a new Jacobian representing one joint.
* 
* @param joint the joint that this Jacobian considers.
* @param jacobianFrame the frame in which the resulting twist of the end effector with respect to the base frame will be expressed.
* @throws RuntimeException if the joint ordering is incorrect.
*/
public GeometricJacobian(InverseDynamicsJoint joint, ReferenceFrame jacobianFrame)
{
 this(joint, joint.getMotionSubspace().getAllUnitTwists(), jacobianFrame);
}

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 setOffDiagonalTerms(int i)
{
 int parentIndex;
 int j = i;
 while (isValidParentIndex(parentIndex = parentMap[j]))
 {
   ReferenceFrame parentFrame = allRigidBodiesInOrder[parentIndex].getInertia().getExpressedInFrame();
   changeFrameOfMomenta(parentFrame);
   j = parentIndex;
   GeometricJacobian motionSubspace = allRigidBodiesInOrder[j].getParentJoint().getMotionSubspace();
   setMassMatrixPart(i, j, motionSubspace);
 }
}

GeometricJacobian motionSubspace = inverseDynamicsJoint.getMotionSubspace();
for (Twist twist : motionSubspace.getAllUnitTwists())

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

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

tempTwist.set(columnJoint.getMotionSubspace().getAllUnitTwists().get(k));
tempTwist.changeFrame(inertia.getExpressedInFrame());
tempMomentum.compute(inertia, tempTwist);

tempTwist.set(jointList[j].getMotionSubspace().getAllUnitTwists().get(k));
tempTwist.changeFrame(rigidBodies[i].getInertia().getExpressedInFrame());