@Test
  public void incorrectInput() {
    init(30, false);

    // compute true essential matrix
    DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());

    // create an alternative incorrect matrix
    Vector3D_F64 T = worldToCamera.getT().copy();
    T.x += 0.1;
    DenseMatrix64F Emod = MultiViewOps.createEssential(worldToCamera.getR(), T);

    ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();

    // compute and compare results
    assertTrue(alg.fitModel(pairs, Emod, found));

    // normalize to allow comparison
    CommonOps.divide(E.get(2, 2), E);
    CommonOps.divide(Emod.get(2, 2), Emod);
    CommonOps.divide(found.get(2, 2), found);

    double error0 = 0;
    double error1 = 0;

    // very crude error metric
    for (int i = 0; i < 9; i++) {
      error0 += Math.abs(Emod.data[i] - E.data[i]);
      error1 += Math.abs(found.data[i] - E.data[i]);
    }

    //		System.out.println("error "+error1+"   other "+error0);
    assertTrue(error1 < error0);
  }
  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);
  }
Beispiel #3
0
 private void init(WeightFunction weightFunction, BasesFunction baseFunction) {
   this.weightFunction = weightFunction;
   this.basesFunction = baseFunction;
   final int baseDim = baseFunction.getDim();
   A = new DenseMatrix64F(baseDim, baseDim);
   A_x = new DenseMatrix64F(baseDim, baseDim);
   A_y = new DenseMatrix64F(baseDim, baseDim);
   As = new DenseMatrix64F[] {A, A_x, A_y};
   B = new ArrayList<>(baseDim);
   B_x = new ArrayList<>(baseDim);
   B_y = new ArrayList<>(baseDim);
   Bs = Arrays.asList(B, B_x, B_y);
   for (ArrayList<TDoubleArrayList> tB : Bs) {
     for (int i = 0; i < baseDim; i++) {
       tB.add(new TDoubleArrayList(ShapeFunctionUtils2D.MAX_NODES_SIZE_GUESS));
     }
   }
   gamma = new DenseMatrix64F(baseDim, 1);
   gamma_x = new DenseMatrix64F(baseDim, 1);
   gamma_y = new DenseMatrix64F(baseDim, 1);
   ps_arr = new double[3][baseDim];
   p = DenseMatrix64F.wrap(ps_arr[0].length, 1, ps_arr[0]);
   p_x = DenseMatrix64F.wrap(ps_arr[1].length, 1, ps_arr[1]);
   p_y = DenseMatrix64F.wrap(ps_arr[2].length, 1, ps_arr[2]);
   tv = new DenseMatrix64F(baseDim, 1);
   luSolver = new LinearSolverLu(new LUDecompositionAlt());
   A_bak = new DenseMatrix64F(baseDim, baseDim);
 }
  private void compute(ValkyrieJointInterface[] jnt_data) {
    double q1 = jnt_data[1].getPosition();
    double q2 = jnt_data[0].getPosition();

    double cq1 = Math.cos(q1);
    double sq1 = Math.sin(q1);

    double cq2 = Math.cos(q2);
    double sq2 = Math.sin(q2);

    double c2q1 = Math.cos(2.0 * q1);
    double c2q2 = Math.cos(2.0 * q2);
    double s2q1 = Math.sin(2.0 * q1);

    double a1 = computeALeft();
    double b1 = computeBLeft(cq1, sq1, cq2, sq2);
    double c1 = computeCLeft(cq1, sq1, cq2, sq2);

    double a2 = computeARight();
    double b2 = computeBRight(cq1, sq1, cq2, sq2);
    double c2 = computeCRight(cq1, sq1, cq2, sq2);

    double Db1Dq1 = computeDb1Dq1(cq1, sq1, sq2);
    double Db1Dq2 = computeDb1Dq2(cq1, cq2, sq2);
    double Dc1Dq1 = computeDc1Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
    double Dc1Dq2 = computeDc1Dq2(cq1, sq1, cq2, sq2, c2q1, s2q1);

    double Db2Dq1 = computeDb2Dq1(cq1, sq1, sq2);
    double Db2Dq2 = computeDb1Dq2(cq1, cq2, sq2);
    double Dc2Dq1 = computeDc2Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
    double Dc2Dq2 = computeDc2Dq2(cq1, sq1, cq2, sq2, c2q1);

    double x1 = computePushrodPosition(a1, b1, c1);
    double x2 = computePushrodPosition(a2, b2, c2);
    pushrodPositions.set(0, 0, x1);
    pushrodPositions.set(1, 0, x2);

    double velocityMap00 = computeVelocityMapCoefficient(Db1Dq1, a1, b1, Dc1Dq1, c1);
    double velocityMap01 = computeVelocityMapCoefficient(Db1Dq2, a1, b1, Dc1Dq2, c1);
    double velocityMap10 = computeVelocityMapCoefficient(Db2Dq1, a2, b2, Dc2Dq1, c2);
    double velocityMap11 = computeVelocityMapCoefficient(Db2Dq2, a2, b2, Dc2Dq2, c2);
    jacobian.set(0, 0, velocityMap00);
    jacobian.set(0, 1, velocityMap01);
    jacobian.set(1, 0, velocityMap10);
    jacobian.set(1, 1, velocityMap11);

    double pushrodForceToJointTorqueMap00 =
        computePushrodForceToJointTorqueMap00(cq1, sq1, sq2, x1);
    double pushrodForceToJointTorqueMap01 =
        computePushrodForceToJointTorqueMap01(cq1, sq1, sq2, x2);
    double pushrodForceToJointTorqueMap10 =
        computePushrodForceToJointTorqueMap10(cq1, sq1, cq2, sq2, x1);
    double pushrodForceToJointTorqueMap11 =
        computePushrodForceToJointTorqueMap11(cq1, sq1, cq2, sq2, x2);
    jacobianTranspose.set(0, 0, pushrodForceToJointTorqueMap00);
    jacobianTranspose.set(0, 1, pushrodForceToJointTorqueMap01);
    jacobianTranspose.set(1, 0, pushrodForceToJointTorqueMap10);
    jacobianTranspose.set(1, 1, pushrodForceToJointTorqueMap11);
  }
  /**
   * Specifies function being optimized.
   *
   * @param function Computes residuals and Jacobian.
   */
  public void setFunction(CoupledJacobian function) {
    internalInitialize(function.getN(), function.getM());
    this.function = function;

    jacobianVals.reshape(M, N);

    B.reshape(N, N);
    Bdiag.reshape(N, 1);
  }
  /**
   * 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++;
  }
Beispiel #7
0
 public static DenseMatrix64F numDiff2d(MatrixChains chains, double[] angles, double epsilon) {
   int l = angles.length;
   DenseMatrix64F togo = new DenseMatrix64F(l, l);
   for (int i = 0; i < l; ++i) {
     for (int j = 0; j < l; ++j) {
       togo.set(i, j, numDiff(chains, angles, i, j, epsilon));
     }
   }
   return togo;
 }
  /**
   * Sets the values in the specified matrix to a rotation matrix about the x-axis.
   *
   * @param ang the angle it rotates a point by in radians.
   * @param R (Output) Storage for rotation matrix. Modified.
   */
  public static void setRotX(float ang, DenseMatrix64F R) {
    float c = (float) Math.cos(ang);
    float s = (float) Math.sin(ang);

    R.set(0, 0, 1);
    R.set(1, 1, c);
    R.set(1, 2, -s);
    R.set(2, 1, s);
    R.set(2, 2, c);
  }
  /**
   * Checks to see if the matrix is or is not modified as according to the modified flag.
   *
   * @param decomp
   */
  public static void checkModifiedInput(DecompositionInterface decomp) {
    DenseMatrix64F A = RandomMatrices.createSymmPosDef(4, new Random(0x434));
    DenseMatrix64F A_orig = A.copy();

    assertTrue(decomp.decompose(A));

    boolean modified = !MatrixFeatures.isEquals(A, A_orig);

    assertTrue(modified + " " + decomp.inputModified(), decomp.inputModified() == modified);
  }
  /**
   * Sets the values in the specified matrix to a rotation matrix about the z-axis.
   *
   * @param ang the angle it rotates a point by in radians.
   * @param r A 3 by 3 matrix. Is modified.
   */
  public static void setRotZ(float ang, DenseMatrix64F r) {
    float c = (float) Math.cos(ang);
    float s = (float) Math.sin(ang);

    r.set(0, 0, c);
    r.set(0, 1, -s);
    r.set(1, 0, s);
    r.set(1, 1, c);
    r.set(2, 2, 1);
  }
  @Override
  public void actuatorToJointEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    pushrodForces.set(0, 0, act_data[0].getEffort());
    pushrodForces.set(1, 0, act_data[1].getEffort());

    //      CommonOps.multTransA(jacobian, pushrodForces, jointTorques);
    CommonOps.mult(jacobianTranspose, pushrodForces, jointTorques);

    jnt_data[1].setEffort(jointTorques.get(0, 0));
    jnt_data[0].setEffort(jointTorques.get(1, 0));
  }
  @Override
  protected MultivariateGaussianDM createPerfectMeas(
      KalmanFilterInterface f, DenseMatrix64F state) {
    KalmanFilter filter = (KalmanFilter) f;
    DenseMatrix64F H = filter.getProjector().getProjectionMatrix();
    DenseMatrix64F X = state;

    DenseMatrix64F z = new DenseMatrix64F(2, 1);

    CommonOps.mult(H, X, z);

    return createState(2.0, z.get(0, 0), z.get(1, 0));
  }
  // ValkyrieJointInterface[] jnt_data = [ pitch, roll ]
  @Override
  public void jointToActuatorEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    jointTorques.set(0, 0, jnt_data[1].getDesiredEffort());
    jointTorques.set(1, 0, jnt_data[0].getDesiredEffort());

    DenseMatrix64F jacobianTransposeInverse = new DenseMatrix64F(2, 2);
    CommonOps.invert(jacobianTranspose, jacobianTransposeInverse);

    CommonOps.mult(jacobianTransposeInverse, jointTorques, pushrodForces);

    act_data[0].setEffortCommand(pushrodForces.get(0, 0));
    act_data[1].setEffortCommand(pushrodForces.get(1, 0));
  }
  /**
   * Converts a vector from eigen space into sample space.
   *
   * @param eigenData Eigen space data.
   * @return Sample space projection.
   */
  public double[] eigenToSampleSpace(double[] eigenData) {
    if (eigenData.length != numComponents)
      throw new IllegalArgumentException("Unexpected sample length");

    DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1);
    DenseMatrix64F r = DenseMatrix64F.wrap(numComponents, 1, eigenData);

    CommonOps.multTransA(V_t, r, s);

    DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);
    CommonOps.add(s, mean, s);

    return s.data;
  }
  @Override
  public void actuatorToJointVelocity(
      LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    pushrodVelocities.set(0, 0, act_data[0].getVelocity());
    pushrodVelocities.set(1, 0, act_data[1].getVelocity());

    DenseMatrix64F jacobianInverse = new DenseMatrix64F(2, 2);
    CommonOps.invert(jacobian, jacobianInverse);
    CommonOps.mult(jacobianInverse, pushrodVelocities, jointVelocites);

    jnt_data[1].setVelocity(-jointVelocites.get(0, 0));
    jnt_data[0].setVelocity(-jointVelocites.get(1, 0));
  }
  /**
   * Converts a vector from sample space into eigen space.
   *
   * @param sampleData Sample space data.
   * @return Eigen space projection.
   */
  public double[] sampleToEigenSpace(double[] sampleData) {
    if (sampleData.length != A.getNumCols())
      throw new IllegalArgumentException("Unexpected sample length");
    DenseMatrix64F mean = DenseMatrix64F.wrap(A.getNumCols(), 1, this.mean);

    DenseMatrix64F s = new DenseMatrix64F(A.getNumCols(), 1, true, sampleData);
    DenseMatrix64F r = new DenseMatrix64F(numComponents, 1);

    CommonOps.sub(s, mean, s);

    CommonOps.mult(V_t, s, r);

    return r.data;
  }
  public static long fillManual(DenseMatrix64F mat, int numTrials) {
    long prev = System.currentTimeMillis();

    for (int i = 0; i < numTrials; i++) {
      final int size = mat.getNumElements();

      for (int j = 0; j < size; j++) {
        mat.set(j, 2);
      }
    }

    long curr = System.currentTimeMillis();
    return curr - prev;
  }
  /**
   * 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;
  }
  /**
   * Extracts quaternions from the provided rotation matrix.
   *
   * @param R (Input) rotation matrix
   * @param quat (Output) Optional storage for quaternion. If null a new class will be used.
   * @return unit quaternion representation of the rotation matrix.
   */
  public static Quaternion_F32 matrixToQuaternion(DenseMatrix64F R, Quaternion_F32 quat) {

    if (quat == null) quat = new Quaternion_F32();

    // algorithm from:
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    //
    // Designed to minimize numerical error by not dividing by very small numbers

    float m00 = (float) R.unsafe_get(0, 0);
    float m01 = (float) R.unsafe_get(0, 1);
    float m02 = (float) R.unsafe_get(0, 2);
    float m10 = (float) R.unsafe_get(1, 0);
    float m11 = (float) R.unsafe_get(1, 1);
    float m12 = (float) R.unsafe_get(1, 2);
    float m20 = (float) R.unsafe_get(2, 0);
    float m21 = (float) R.unsafe_get(2, 1);
    float m22 = (float) R.unsafe_get(2, 2);

    float trace = m00 + m11 + m22;

    if (trace > 0) {
      float S = (float) Math.sqrt(trace + 1.0f) * 2; // S=4*qw
      quat.w = 0.25f * S;
      quat.x = (m21 - m12) / S;
      quat.y = (m02 - m20) / S;
      quat.z = (m10 - m01) / S;
    } else if ((m00 > m11) & (m00 > m22)) {
      float S = (float) Math.sqrt(1.0f + m00 - m11 - m22) * 2; // S=4*qx
      quat.w = (m21 - m12) / S;
      quat.x = 0.25f * S;
      quat.y = (m01 + m10) / S;
      quat.z = (m02 + m20) / S;
    } else if (m11 > m22) {
      float S = (float) Math.sqrt(1.0f + m11 - m00 - m22) * 2; // S=4*qy
      quat.w = (m02 - m20) / S;
      quat.x = (m01 + m10) / S;
      quat.y = 0.25f * S;
      quat.z = (m12 + m21) / S;
    } else {
      float S = (float) Math.sqrt(1.0f + m22 - m00 - m11) * 2; // S=4*qz
      quat.w = (m10 - m01) / S;
      quat.x = (m02 + m20) / S;
      quat.y = (m12 + m21) / S;
      quat.z = 0.25f * S;
    }

    return quat;
  }
Beispiel #20
0
  /**
   * Loads the word-vector pairs stored in the file whose path is
   * <code>filename</code> The file can be a plain text file or a .gz archive.
   * <p>
   * The expected format is: </br>
   * number_of_vectors space_dimensionality</br>
   * word_i [TAB] 1.0 [TAB] 0 [TAB] vector values comma
   * separated </code> </br>
   * </br>
   * Example: </br>
   * </br>
   * <code> 3 5</br>
   * dog::n [TAB] 1.0 [TAB] 0 [TAB] 2.1,4.1,1.4,2.3,0.9</br>
   * cat::n [TAB] 1.0 [TAB] 0 [TAB] 3.2,4.3,1.2,2.2,0.8</br>
   * mouse::n [TAB] 1.0 [TAB] 0 [TAB] 2.4,4.4,2.4,1.3,0.92</br>
   *
   *
   * @param filename
   *            the path of the file containing the word-vector pairs
   * @throws IOException
   */
  private void populate(String filename) throws IOException {
    BufferedReader br = null;
    GZIPInputStream gzis = null;
    if (filename.endsWith(".gz")) {
      gzis = new GZIPInputStream(new FileInputStream(filename));
      InputStreamReader reader = new InputStreamReader(gzis, "UTF8");
      br = new BufferedReader(reader);
    } else {
      br = new BufferedReader(new InputStreamReader(new FileInputStream(filename), "UTF8"));
    }
    String line;
    ArrayList<String> split;
    String label;
    String[] vSplit;

    Pattern iPattern = Pattern.compile(",");
    float[] v = null;

    while ((line = br.readLine()) != null) {
      if (!line.contains("\t")) continue;
      float norm2 = 0;
      split = mySplit(line);
      label = split.get(0);
      vSplit = iPattern.split(split.get(3), 0);
      if (v == null) v = new float[vSplit.length];
      for (int i = 0; i < v.length; i++) {
        v[i] = Float.parseFloat(vSplit[i]);
        norm2 += v[i] * v[i];
      }
      float norm = (float) Math.sqrt(norm2);
      for (int i = 0; i < v.length; i++) {
        v[i] /= norm;
      }

      DenseMatrix64F featureVector = new DenseMatrix64F(1, v.length);
      for (int i = 0; i < v.length; i++) {
        featureVector.set(0, i, (double) v[i]);
      }

      DenseVector denseFeatureVector = new DenseVector(featureVector);

      addWordVector(label, denseFeatureVector);
    }
    if (filename.endsWith(".gz")) {
      gzis.close();
    }
    br.close();
  }
 public DenseMatrix64F getCentroidalMomentumMatrixPart(InverseDynamicsJoint[] joints) {
   int partDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(joints);
   centroidalMomentumMatrixPart.reshape(Momentum.SIZE, partDegreesOfFreedom);
   centroidalMomentumMatrixPart.zero();
   int startColumn = 0;
   for (InverseDynamicsJoint joint : joints) {
     int[] columnsForJoint = columnsForJoints.get(joint);
     MatrixTools.extractColumns(
         centroidalMomentumRateTermCalculator.getCentroidalMomentumMatrix(),
         columnsForJoint,
         centroidalMomentumMatrixPart,
         startColumn);
     startColumn += columnsForJoint.length;
   }
   return centroidalMomentumMatrixPart;
 }
 public void setDesiredAcceleration(
     FrameVector angularAcceleration, FrameVector linearAcceleration) {
   desiredAcceleration.reshape(6, 1);
   MatrixTools.insertTuple3dIntoEJMLVector(
       angularAcceleration.getVector(), desiredAcceleration, 0);
   MatrixTools.insertTuple3dIntoEJMLVector(linearAcceleration.getVector(), desiredAcceleration, 3);
 }
Beispiel #23
0
  // Produce 2nd derivative of objective function at specified angle setting
  public static DenseMatrix64F get2d(MatrixChains chain, double[] angles) {
    if (angles == null) // yingdan
    return null;

    int dim = angles.length;
    DenseMatrix64F togo = new DenseMatrix64F(dim, dim);
    for (int i = 0; i < dim; ++i) {
      for (int j = 0; j < dim; ++j) {
        double v = df2by(chain, angles, i, j);
        if (chain.regularise > 0 && i == j) {
          v += 2 * chain.regularise;
        }
        togo.set(i, j, v);
      }
    }
    return togo;
  }
  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();
  }
  @Test
  public void perfectInput() {
    init(30, false);

    // compute true essential matrix
    DenseMatrix64F E = MultiViewOps.createEssential(worldToCamera.getR(), worldToCamera.getT());

    ModelFitter<DenseMatrix64F, AssociatedPair> alg = createAlgorithm();

    // give it the perfect matrix and see if it screwed it up
    assertTrue(alg.fitModel(pairs, E, found));

    // normalize so that they are the same
    CommonOps.divide(E.get(2, 2), E);
    CommonOps.divide(found.get(2, 2), found);

    assertTrue(MatrixFeatures.isEquals(E, found, 1e-8));
  }
 /** 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++);
   }
 }
  private static void computeLowerCoef(
      WlCoef_F32 inverse, DenseMatrix64F a_inv, WlBorderCoefFixed ret, int col) {
    int lengthWavelet = inverse.wavelet.length + inverse.offsetWavelet + col;
    int lengthScaling = inverse.scaling.length + inverse.offsetScaling + col;
    lengthWavelet = Math.min(lengthWavelet, inverse.wavelet.length);
    lengthScaling = Math.min(lengthScaling, inverse.scaling.length);

    float[] coefScaling = new float[lengthScaling];
    float[] coefWavelet = new float[lengthWavelet];

    for (int j = 0; j < lengthScaling; j++) {
      coefScaling[j] = (float) a_inv.get(j, col);
    }
    for (int j = 0; j < lengthWavelet; j++) {
      coefWavelet[j] = (float) a_inv.get(j, col + 1);
    }
    ret.lowerCoef[col] = new WlCoef_F32(coefScaling, 0, coefWavelet, 0);
  }
Beispiel #28
0
  public static OptimisationReport NiuDunLaFuSunStep(
      MatrixChains chain, double[] angles, boolean printStep) {
    // see: http://code.google.com/p/efficient-java-matrix-library/wiki/SolvingLinearSystems
    int l = angles.length;
    DenseMatrix64F J = get2d(chain, angles);

    DenseMatrix64F grad = get1d(chain, angles);
    DenseMatrix64F propose = vector(l);

    boolean solved = CommonOps.solve(J, grad, propose);
    for (int i = 0; i < l; ++i) {
      if (Double.isNaN(propose.data[i])) {
        solved = false;
      }
    }
    OptimisationReport togo = new OptimisationReport();

    try {
      if (!solved) {
        throw new RuntimeException("Failed to solve update equation");
      }
      CommonOps.scale(-1, propose);

      if (printStep) {
        EigenDecomposition<DenseMatrix64F> decomp = DecompositionFactory.eigSymm(l, false);
        boolean posed = decomp.decompose(J);
        if (!posed) {
          throw new RuntimeException("Failed to decompose matrix");
        }
        double[] eigs = eigs(decomp);
        System.out.println("Computed eigenvalues " + printVec(eigs));
        togo.eigenvalues = eigs;
      }
      tryProposal(chain, propose, grad, angles, printStep, togo);
    } catch (Exception e) {
      System.out.println(
          "Failed to find suitable proposal from Newton step - trying gradient step");
      propose.set(grad);
      CommonOps.scale(-1, propose);
      tryProposal(chain, propose, grad, angles, printStep, togo);
    }
    return togo;
  }
Beispiel #29
0
  /**
   * Converts this matrix formated trifocal into a 27 element vector:<br>
   * m.data[ i*9 + j*3 + k ] = T_i(j,k)
   *
   * @param m Output: Trifocal tensor encoded in a vector
   */
  public void convertTo(DenseMatrix64F m) {
    if (m.getNumElements() != 27)
      throw new IllegalArgumentException("Input matrix/vector must have 27 elements");

    for (int i = 0; i < 9; i++) {
      m.data[i] = T1.data[i];
      m.data[i + 9] = T2.data[i];
      m.data[i + 18] = T3.data[i];
    }
  }
  public static long fillArrays(DenseMatrix64F mat, int numTrials) {
    long prev = System.currentTimeMillis();

    for (int i = 0; i < numTrials; i++) {
      Arrays.fill(mat.data, 0, mat.getNumElements(), 2);
    }

    long curr = System.currentTimeMillis();
    return curr - prev;
  }