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