public void compute() {
    centroidalMomentumMatrix.compute();
    MatrixYoVariableConversionTools.getFromYoVariables(
        previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);

    MatrixTools.numericallyDifferentiate(
        centroidalMomentumMatrixDerivative,
        previousCentroidalMomentumMatrix,
        centroidalMomentumMatrix.getMatrix(),
        controlDT);
    MatrixYoVariableConversionTools.storeInYoVariables(
        previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);

    ScrewTools.packJointVelocitiesMatrix(jointsInOrder, v);
    CommonOps.mult(centroidalMomentumMatrixDerivative, v, adotV);
  }
  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);
  }
  public MomentumSolver3(
      SixDoFJoint rootJoint,
      RigidBody elevator,
      ReferenceFrame centerOfMassFrame,
      TwistCalculator twistCalculator,
      LinearSolver<DenseMatrix64F> jacobianSolver,
      double controlDT,
      YoVariableRegistry parentRegistry) {
    this.rootJoint = rootJoint;
    this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootJoint.getSuccessor());

    this.motionConstraintHandler =
        new MotionConstraintHandler("solver3", jointsInOrder, twistCalculator, null, registry);

    this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
    this.previousCentroidalMomentumMatrix =
        new DenseMatrix64F(
            centroidalMomentumMatrix.getMatrix().getNumRows(),
            centroidalMomentumMatrix.getMatrix().getNumCols());
    this.centroidalMomentumMatrixDerivative =
        new DenseMatrix64F(
            centroidalMomentumMatrix.getMatrix().getNumRows(),
            centroidalMomentumMatrix.getMatrix().getNumCols());
    yoPreviousCentroidalMomentumMatrix =
        new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
            [previousCentroidalMomentumMatrix.getNumCols()];
    MatrixYoVariableConversionTools.populateYoVariables(
        yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);

    this.controlDT = controlDT;

    nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
    this.b = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
    this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);

    //      nullspaceMotionConstraintEnforcer = new
    // EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
    equalityConstraintEnforcer =
        new EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));

    solver = LinearSolverFactory.pseudoInverse(true);

    parentRegistry.addChild(registry);
    reset();
  }
 public void initialize() {
   centroidalMomentumMatrix.compute();
   previousCentroidalMomentumMatrix.set(centroidalMomentumMatrix.getMatrix());
   MatrixYoVariableConversionTools.storeInYoVariables(
       previousCentroidalMomentumMatrix, yoPreviousCentroidalMomentumMatrix);
 }