public CentroidalMomentumHandler(RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry)
{
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame);
this.previousCentroidalMomentumMatrix = new DenseMatrix64F(centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix = new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()][previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
for (InverseDynamicsJoint joint : jointsInOrder)
{
TIntArrayList listToPackIndices = new TIntArrayList();
ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint);
int[] indices = listToPackIndices.toArray();
columnsForJoints.put(joint, indices);
}
centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame);
this.centerOfMassFrame = centerOfMassFrame;
parentRegistry.addChild(registry);
double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
this.centroidalMomentumRateTermCalculator = new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass);
}