How to use

getConfigurationMatrix

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

private void getConfigurationDataFromJoints(InverseDynamicsJoint[] joints)
{
 for (int i = 0; i < joints.length; i++)
 {
   InverseDynamicsJoint joint = joints[i];
   DenseMatrix64F jointConfiguration = jointConfigurations[i];
   if (jointConfiguration == null)
   {
    jointConfiguration = new DenseMatrix64F(joint.getConfigurationMatrixSize(), 1);
    jointConfigurations[i] = jointConfiguration;
   }
   joint.getConfigurationMatrix(jointConfiguration, 0);
 }
}

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