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);
  }
  /**
   * Adds a new sample of the raw data to internal data structure for later processing. All the
   * samples must be added before computeBasis is called.
   *
   * @param sampleData Sample from original raw data.
   */
  public void addSample(double[] sampleData) {
    if (A.getNumCols() != sampleData.length)
      throw new IllegalArgumentException("Unexpected sample size");
    if (sampleIndex >= A.getNumRows()) throw new IllegalArgumentException("Too many samples");

    for (int i = 0; i < sampleData.length; i++) {
      A.set(sampleIndex, i, sampleData[i]);
    }
    sampleIndex++;
  }
  /**
   * Computes a basis (the principle components) from the most dominant eigenvectors.
   *
   * @param numComponents Number of vectors it will use to describe the data. Typically much smaller
   *     than the number of elements in the input vector.
   */
  public void computeBasis(int numComponents) {
    if (numComponents > A.getNumCols())
      throw new IllegalArgumentException("More components requested that the data's length.");
    if (sampleIndex != A.getNumRows())
      throw new IllegalArgumentException("Not all the data has been added");
    if (numComponents > sampleIndex)
      throw new IllegalArgumentException(
          "More data needed to compute the desired number of components");

    this.numComponents = numComponents;

    // compute the mean of all the samples
    for (int i = 0; i < A.getNumRows(); i++) {
      for (int j = 0; j < mean.length; j++) {
        mean[j] += A.get(i, j);
      }
    }
    for (int j = 0; j < mean.length; j++) {
      mean[j] /= A.getNumRows();
    }

    // subtract the mean from the original data
    for (int i = 0; i < A.getNumRows(); i++) {
      for (int j = 0; j < mean.length; j++) {
        A.set(i, j, A.get(i, j) - mean[j]);
      }
    }

    // Compute SVD and save time by not computing U
    SingularValueDecomposition<DenseMatrix64F> svd =
        DecompositionFactory.svd(A.numRows, A.numCols, false, true, false);
    if (!svd.decompose(A)) throw new RuntimeException("SVD failed");

    V_t = svd.getV(null, true);
    DenseMatrix64F W = svd.getW(null);

    // Singular values are in an arbitrary order initially
    SingularOps.descendingOrder(null, false, W, V_t, true);

    // strip off unneeded components and find the basis
    V_t.reshape(numComponents, mean.length, true);
  }
  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();
  }
 /** Computes the Jacobian. */
 public void compute() {
   int column = 0;
   for (int i = 0; i < unitTwistList.size(); i++) {
     Twist twist = unitTwistList.get(i);
     tempTwist.set(twist);
     tempTwist.changeFrame(jacobianFrame);
     tempTwist.getMatrix(tempMatrix, 0);
     CommonOps.extract(
         tempMatrix,
         0,
         tempMatrix.getNumRows(),
         0,
         tempMatrix.getNumCols(),
         jacobian,
         0,
         column++);
   }
 }
Exemple #6
0
 public int size() {
   return values.getNumRows();
 }
Exemple #7
0
 public Tuple(DenseMatrix64F values) {
   // values is a column matrix
   this.values = values;
   this.dimensions = values.getNumRows();
 }
  public void setDesiredAcceleration(DenseMatrix64F qdd) {
    if (qdd.getNumRows() != 6 || qdd.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + qdd);

    desiredAcceleration.set(qdd);
  }
  public void setDesiredVelocity(DenseMatrix64F qd) {
    if (qd.getNumRows() != 6 || qd.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + qd);

    desiredVelocity.set(qd);
  }
  public void setDesiredConfiguration(DenseMatrix64F q) {
    if (q.getNumRows() != 7 || q.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + q);

    desiredConfiguration.set(q);
  }
 @Override
 public boolean hasDesiredAcceleration() {
   return desiredAcceleration.getNumRows() != 0;
 }
 @Override
 public boolean hasDesiredVelocity() {
   return desiredVelocity.getNumRows() != 0;
 }
 @Override
 public boolean hasDesiredConfiguration() {
   return desiredConfiguration.getNumRows() != 0;
 }
  public void solve(
      DenseMatrix64F accelerationSubspace,
      DenseMatrix64F accelerationMultipliers,
      DenseMatrix64F momentumSubspace,
      DenseMatrix64F momentumMultipliers) {
    if (accelerationSubspace.getNumCols() > 0) {
      TaskspaceConstraintData rootJointTaskspaceConstraintData = new TaskspaceConstraintData();
      DenseMatrix64F rootJointAccelerationMatrix = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
      CommonOps.mult(accelerationSubspace, accelerationMultipliers, rootJointAccelerationMatrix);
      SpatialAccelerationVector spatialAcceleration =
          new SpatialAccelerationVector(
              rootJoint.getFrameAfterJoint(),
              rootJoint.getFrameBeforeJoint(),
              rootJoint.getFrameAfterJoint(),
              rootJointAccelerationMatrix);
      spatialAcceleration.changeBodyFrameNoRelativeAcceleration(
          rootJoint.getSuccessor().getBodyFixedFrame());
      spatialAcceleration.changeFrameNoRelativeMotion(rootJoint.getSuccessor().getBodyFixedFrame());
      DenseMatrix64F nullspaceMultipliers = new DenseMatrix64F(0, 1);
      DenseMatrix64F selectionMatrix =
          new DenseMatrix64F(accelerationSubspace.getNumCols(), accelerationSubspace.getNumRows());
      CommonOps.transpose(accelerationSubspace, selectionMatrix);
      rootJointTaskspaceConstraintData.set(
          spatialAcceleration, nullspaceMultipliers, selectionMatrix);
      setDesiredSpatialAcceleration(
          rootJoint.getMotionSubspace(), rootJointTaskspaceConstraintData);
    }

    // sTranspose
    sTranspose.reshape(momentumSubspace.getNumCols(), momentumSubspace.getNumRows());
    CommonOps.transpose(momentumSubspace, sTranspose);

    // b
    b.reshape(sTranspose.getNumRows(), 1);
    b.set(momentumMultipliers);
    CommonOps.multAdd(-1.0, sTranspose, adotV, b);

    // sTransposeA
    sTransposeA.reshape(sTranspose.getNumRows(), centroidalMomentumMatrix.getMatrix().getNumCols());
    CommonOps.mult(sTranspose, centroidalMomentumMatrix.getMatrix(), sTransposeA);

    // assemble Jp, pp
    motionConstraintHandler.compute();
    DenseMatrix64F Jp = motionConstraintHandler.getJacobian();
    DenseMatrix64F pp = motionConstraintHandler.getRightHandSide();

    //      DenseMatrix64F n = motionConstraintHandler.getNullspaceMatrixTranspose();
    //      DenseMatrix64F z = motionConstraintHandler.getNullspaceMultipliers();

    //      nullspaceMotionConstraintEnforcer.set(n, z);
    //      nullspaceMotionConstraintEnforcer.constrainEquation(Jp, pp);

    equalityConstraintEnforcer.setConstraint(Jp, pp);
    //      nullspaceMotionConstraintEnforcer.constrainEquation(sTransposeA, b);
    equalityConstraintEnforcer.constrainEquation(sTransposeA, b);

    // APPlusbMinusAJpPluspp
    vdotUnconstrained.reshape(nDegreesOfFreedom, 1);
    solver.setA(sTransposeA);
    solver.solve(b, vdotUnconstrained);

    DenseMatrix64F vdot = equalityConstraintEnforcer.constrainResult(vdotUnconstrained);
    //      DenseMatrix64F vdot = nullspaceMotionConstraintEnforcer.constrainResult(vdot);

    ScrewTools.setDesiredAccelerations(jointsInOrder, vdot);

    CommonOps.mult(centroidalMomentumMatrix.getMatrix(), vdot, hdot);
    CommonOps.addEquals(hdot, adotV);
  }
 public void setSelectionMatrix(DenseMatrix64F selectionMatrix) {
   this.selectionMatrix.reshape(selectionMatrix.getNumRows(), selectionMatrix.getNumCols());
   this.selectionMatrix.set(selectionMatrix);
 }