How to use

getDegreesOfFreedom

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

public static int computeDegreesOfFreedom(Iterable<? extends InverseDynamicsJoint> jointList)
{
 int ret = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   ret += joint.getDegreesOfFreedom();
 }
 return ret;
}

public static int computeDegreesOfFreedom(InverseDynamicsJoint[] jointList)
{
 int ret = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   ret += joint.getDegreesOfFreedom();
 }
 return ret;
}

public static int computeDegreesOfFreedom(List<? extends InverseDynamicsJoint> jointList)
{
 int ret = 0;
 for (int i = 0; i < jointList.size(); i++)
 {
   ret += jointList.get(i).getDegreesOfFreedom();
 }
 return ret;
}

public static void getJointVelocitiesMatrix(Iterable<? extends InverseDynamicsJoint> joints, DenseMatrix64F jointVelocitiesMatrixToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   int dof = joint.getDegreesOfFreedom();
   joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart);
   rowStart += dof;
 }
}

private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder)
{
 int[] ret = new int[rigidBodiesInOrder.length];
 int currentIndex = 0;
 for (int i = 0; i < rigidBodiesInOrder.length; i++)
 {
   RigidBody rigidBody = rigidBodiesInOrder[i];
   ret[i] = currentIndex;
   currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom();
 }
 return ret;
}

public static void getJointVelocitiesMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F jointVelocitiesMatrixToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   int dof = joint.getDegreesOfFreedom();
   joint.getVelocityMatrix(jointVelocitiesMatrixToPack, rowStart);
   rowStart += dof;
 }
}

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

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

public static void getDesiredJointAccelerationsMatrix(InverseDynamicsJoint[] joints, DenseMatrix64F desiredJointAccelerationsMatrixToPack)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : joints)
 {
   int dof = joint.getDegreesOfFreedom();
   joint.getDesiredAccelerationMatrix(desiredJointAccelerationsMatrixToPack, rowStart);
   rowStart += dof;
 }
}

public static void setVelocities(InverseDynamicsJoint[] jointList, DenseMatrix64F jointVelocities)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   joint.setVelocity(jointVelocities, rowStart);
   rowStart += joint.getDegreesOfFreedom();
 }
}

public static void setDesiredAccelerations(InverseDynamicsJoint[] jointList, DenseMatrix64F jointAccelerations)
{
 int rowStart = 0;
 for (InverseDynamicsJoint joint : jointList)
 {
   joint.setDesiredAcceleration(jointAccelerations, rowStart);
   rowStart += joint.getDegreesOfFreedom();
 }
}

private static int determineSize(RigidBody[] rigidBodiesInOrder)
{
 int ret = 0;
 for (RigidBody rigidBody : rigidBodiesInOrder)
 {
   ret += rigidBody.getParentJoint().getDegreesOfFreedom();
 }
 return ret;
}

public static void computeIndexForJoint(InverseDynamicsJoint[] jointsInOrder, TIntArrayList listToPackIndices, InverseDynamicsJoint jointToComputeIndicesFor)
{
 int startIndex = 0;
 for (int i = 0; i < jointsInOrder.length; i++)
 {
   int nDegreesOfFreedom = jointsInOrder[i].getDegreesOfFreedom();
   if (jointsInOrder[i] == jointToComputeIndicesFor)
   {
    for (int k = startIndex; k < startIndex + nDegreesOfFreedom; k++)
    {
      listToPackIndices.add(k);
    }
   }
   startIndex += nDegreesOfFreedom;
 }
}

private void checkConsistency(InverseDynamicsJoint joint, DenseMatrix64F desiredVelocity)
{
 MathTools.checkIfEqual(joint.getDegreesOfFreedom(), desiredVelocity.getNumRows());
}

private void checkConsistency(InverseDynamicsJoint joint, DenseMatrix64F desiredAcceleration)
{
 MathTools.checkIfEqual(joint.getDegreesOfFreedom(), desiredAcceleration.getNumRows());
}

public static void setRandomVelocities(InverseDynamicsJoint[] joints, Random random)
{
 for (InverseDynamicsJoint joint : joints)
 {
   DenseMatrix64F jointVelocity = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1);
   RandomMatrices.setRandom(jointVelocity, random);
   joint.setVelocity(jointVelocity, 0);
 }
}

private void setDesiredAccelerationsToZero()
{
 for (InverseDynamicsJoint joint : jointsInOrder)
 {
   joint.setDesiredAccelerationToZero();
   joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0);
 }
}

public void setOneDoFJointDesiredAcceleration(int jointIndex, double desiredVelocity)
{
 MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1);
 desiredVelocities.get(jointIndex).reshape(1, 1);
 desiredVelocities.get(jointIndex).set(0, 0, desiredVelocity);
}

public void setOneDoFJointDesiredAcceleration(int jointIndex, double desiredAcceleration)
{
 MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1);
 desiredAccelerations.get(jointIndex).reshape(1, 1);
 desiredAccelerations.get(jointIndex).set(0, 0, desiredAcceleration);
}

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