How to use

setConfiguration

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

public void putConfigurationsIntoJoints(InverseDynamicsJoint[] joints)
{
 if (jointConfigurations.length != joints.length)
   throw new RuntimeException("Number of joints does not match up. Received: " + jointConfigurations.length + ", expected: " + joints.length);
 for (int i = 0; i < jointConfigurations.length; i++)
 {
   DenseMatrix64F configuration = jointConfigurations[i];
   joints[i].setConfiguration(configuration, 0);
 }
}

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