How to use

getTauMatrix

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

public static DenseMatrix64F getTauMatrix(InverseDynamicsJoint[] jointsInOrder)
{
 int size = 0;
 for (InverseDynamicsJoint joint : jointsInOrder)
 {
   size += joint.getDegreesOfFreedom();
 }
 DenseMatrix64F tempMatrix = new DenseMatrix64F(InverseDynamicsJoint.maxDoF, 1);
 DenseMatrix64F ret = new DenseMatrix64F(size, 1);
 int startIndex = 0;
 for (InverseDynamicsJoint joint : jointsInOrder)
 {
   int endIndex = startIndex + joint.getDegreesOfFreedom() - 1;
   joint.getTauMatrix(tempMatrix);
   MatrixTools.setMatrixBlock(ret, startIndex, 0, tempMatrix, 0, 0, joint.getDegreesOfFreedom(), 1, 1.0);
   startIndex = endIndex + 1;
 }
 return ret;
}

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