/**
   * Computes inverse coefficients
   *
   * @param border
   * @param forward Forward coefficients.
   * @param inverse Inverse used in the inner portion of the data stream.
   * @return
   */
  private static WlBorderCoef<WlCoef_F32> computeBorderCoefficients(
      BorderIndex1D border, WlCoef_F32 forward, WlCoef_F32 inverse) {
    int N = Math.max(forward.getScalingLength(), forward.getWaveletLength());
    N += N % 2;
    N *= 2;
    border.setLength(N);

    // Because the wavelet transform is a linear invertible system the inverse coefficients
    // can be found by creating a matrix and inverting the matrix.  Boundary conditions are then
    // extracted from this inverted matrix.
    DenseMatrix64F A = new DenseMatrix64F(N, N);
    for (int i = 0; i < N; i += 2) {

      for (int j = 0; j < forward.scaling.length; j++) {
        int index = border.getIndex(j + i + forward.offsetScaling);
        A.add(i, index, forward.scaling[j]);
      }

      for (int j = 0; j < forward.wavelet.length; j++) {
        int index = border.getIndex(j + i + forward.offsetWavelet);
        A.add(i + 1, index, forward.wavelet[j]);
      }
    }

    LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.linear(N);
    if (!solver.setA(A) || solver.quality() < 1e-5) {
      throw new IllegalArgumentException("Can't invert matrix");
    }

    DenseMatrix64F A_inv = new DenseMatrix64F(N, N);
    solver.invert(A_inv);

    int numBorder = UtilWavelet.borderForwardLower(inverse) / 2;

    WlBorderCoefFixed<WlCoef_F32> ret = new WlBorderCoefFixed<>(numBorder, numBorder + 1);
    ret.setInnerCoef(inverse);

    // add the lower coefficients first
    for (int i = 0; i < ret.getLowerLength(); i++) {
      computeLowerCoef(inverse, A_inv, ret, i * 2);
    }

    // add upper coefficients
    for (int i = 0; i < ret.getUpperLength(); i++) {
      computeUpperCoef(inverse, N, A_inv, ret, i * 2);
    }

    return ret;
  }
Beispiel #2
0
  /**
   * Computes the most dominant eigen vector of A using an inverted shifted matrix. The inverted
   * shifted matrix is defined as <b>B = (A - &alpha;I)<sup>-1</sup></b> and can converge faster if
   * &alpha; is chosen wisely.
   *
   * @param A An invertible square matrix matrix.
   * @param alpha Shifting factor.
   * @return If it converged or not.
   */
  public boolean computeShiftInvert(DenseMatrix64F A, double alpha) {
    initPower(A);

    LinearSolver solver = LinearSolverFactory.linear(A.numCols);

    SpecializedOps.addIdentity(A, B, -alpha);
    solver.setA(B);

    boolean converged = false;

    for (int i = 0; i < maxIterations && !converged; i++) {
      solver.solve(q0, q1);
      double s = NormOps.normPInf(q1);
      CommonOps.divide(q1, s, q2);

      converged = checkConverged(A);
    }

    return converged;
  }
  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);
  }