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);
  }
コード例 #2
0
  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();
  }