private void solveUsingTriangle(double real, int index, DenseMatrix64F r ) { for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,-real); } SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); TriangularSolver.solveU(implicit.A.data,r.data,implicit.A.numRows,0,index); for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,real); } }
private void solveUsingTriangle(double real, int index, DenseMatrix64F r ) { for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,-real); } SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); TriangularSolver.solveU(implicit.A.data,r.data,implicit.A.numRows,0,index); for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,real); } }
private void solveUsingTriangle(double real, int index, DenseMatrix64F r ) { for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,-real); } SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); TriangularSolver.solveU(implicit.A.data,r.data,implicit.A.numRows,0,index); for( int i = 0; i < index; i++ ) { implicit.A.add(i,i,real); } }
public static void addDiagonal(DenseMatrix64F matrix, double scalar) { int n = Math.max(matrix.getNumRows(), matrix.getNumCols()); for (int i = 0; i < n; i++) { matrix.add(i, i, scalar); } }
public static void addDiagonal(DenseMatrix64F matrix, double scalar) { int n = Math.max(matrix.getNumRows(), matrix.getNumCols()); for (int i = 0; i < n; i++) { matrix.add(i, i, scalar); } }
@Override public void predict() { rotationVector.setElement(0, stateVector.get(angularVelocityStart + 0)); rotationVector.setElement(1, stateVector.get(angularVelocityStart + 1)); rotationVector.setElement(2, stateVector.get(angularVelocityStart + 2)); orientation.transform(rotationVector); linearVelocity.set(linearVelocityStart, stateVector); orientation.transform(linearVelocity); rotationVector.scale(dt); add(orientation, rotationVector); stateVector.add(angularVelocityStart + 0, 0, dt * stateVector.get(angularAccelerationStart + 0)); stateVector.add(angularVelocityStart + 1, 0, dt * stateVector.get(angularAccelerationStart + 1)); stateVector.add(angularVelocityStart + 2, 0, dt * stateVector.get(angularAccelerationStart + 2)); stateVector.add(positionStart + 0, 0, dt * linearVelocity.getElement(0)); stateVector.add(positionStart + 1, 0, dt * linearVelocity.getElement(1)); stateVector.add(positionStart + 2, 0, dt * linearVelocity.getElement(2)); stateVector.add(linearVelocityStart + 0, 0, dt * stateVector.get(linearAccelerationStart + 0)); stateVector.add(linearVelocityStart + 1, 0, dt * stateVector.get(linearAccelerationStart + 1)); stateVector.add(linearVelocityStart + 2, 0, dt * stateVector.get(linearAccelerationStart + 2)); }
/** * Creates a random symmetric positive definite matrix. * * @param width The width of the square matrix it returns. * @param rand Random number generator used to make the matrix. * @return The random symmetric positive definite matrix. */ public static DenseMatrix64F createSymmPosDef(int width, Random rand) { // This is not formally proven to work. It just seems to work. DenseMatrix64F a = new DenseMatrix64F(width,1); DenseMatrix64F b = new DenseMatrix64F(width,width); for( int i = 0; i < width; i++ ) { a.set(i,0,rand.nextDouble()); } CommonOps.multTransB(a,a,b); for( int i = 0; i < width; i++ ) { b.add(i,i,1); } return b; }
/** * Creates a random symmetric positive definite matrix. * * @param width The width of the square matrix it returns. * @param rand Random number generator used to make the matrix. * @return The random symmetric positive definite matrix. */ public static DenseMatrix64F createSymmPosDef(int width, Random rand) { // This is not formally proven to work. It just seems to work. DenseMatrix64F a = new DenseMatrix64F(width,1); DenseMatrix64F b = new DenseMatrix64F(width,width); for( int i = 0; i < width; i++ ) { a.set(i,0,rand.nextDouble()); } CommonOps.multTransB(a,a,b); for( int i = 0; i < width; i++ ) { b.add(i,i,1); } return b; }
/** * Creates a random symmetric positive definite matrix. * * @param width The width of the square matrix it returns. * @param rand Random number generator used to make the matrix. * @return The random symmetric positive definite matrix. */ public static DenseMatrix64F createSymmPosDef(int width, Random rand) { // This is not formally proven to work. It just seems to work. DenseMatrix64F a = new DenseMatrix64F(width,1); DenseMatrix64F b = new DenseMatrix64F(width,width); for( int i = 0; i < width; i++ ) { a.set(i,0,rand.nextDouble()); } CommonOps.multTransB(a,a,b); for( int i = 0; i < width; i++ ) { b.add(i,i,1); } return b; }
private void solveWithLU(double real, int index, DenseMatrix64F r ) { DenseMatrix64F A = new DenseMatrix64F(index,index); CommonOps.extract(implicit.A,0,index,0,index,A,0,0); for( int i = 0; i < index; i++ ) { A.add(i,i,-real); } r.reshape(index,1, false); SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); // TODO this must be very inefficient if( !solver.setA(A)) throw new RuntimeException("Solve failed"); solver.solve(r,r); }
private void solveWithLU(double real, int index, DenseMatrix64F r ) { DenseMatrix64F A = new DenseMatrix64F(index,index); CommonOps.extract(implicit.A,0,index,0,index,A,0,0); for( int i = 0; i < index; i++ ) { A.add(i,i,-real); } r.reshape(index,1, false); SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); // TODO this must be very inefficient if( !solver.setA(A)) throw new RuntimeException("Solve failed"); solver.solve(r,r); }
private void solveWithLU(double real, int index, DenseMatrix64F r ) { DenseMatrix64F A = new DenseMatrix64F(index,index); CommonOps.extract(implicit.A,0,index,0,index,A,0,0); for( int i = 0; i < index; i++ ) { A.add(i,i,-real); } r.reshape(index,1, false); SpecializedOps.subvector(implicit.A,0,index,index,false,0,r); CommonOps.changeSign(r); // TODO this must be very inefficient if( !solver.setA(A)) throw new RuntimeException("Solve failed"); solver.solve(r,r); }
public void updateIntermediateVariables() { if (articulatedBodyRecursionStep.isRoot()) return; if (isUpToDate) return; CommonOps.multTransB(articulatedBodyRecursionStep.U_Dinv, articulatedBodyRecursionStep.S, U_Dinv_ST); for (int index = 0; index < U_Dinv_ST.getNumElements(); index++) one_minus_U_Dinv_ST.set(index, -U_Dinv_ST.get(index)); for (int diagIndex = 0; diagIndex < SpatialVectorReadOnly.SIZE; diagIndex++) one_minus_U_Dinv_ST.add(diagIndex, diagIndex, 1.0); CommonOps.multTransB(articulatedBodyRecursionStep.Dinv, articulatedBodyRecursionStep.U, Dinv_UT); CommonOps.multTransB(articulatedBodyRecursionStep.Dinv, articulatedBodyRecursionStep.S, Dinv_ST); isUpToDate = true; }
/** * Compute the nullspace of the given matrix as follows: * <p> * Ν = I - M<sup>+</sup>M * </p> * Where M<sup>+</sup> is the Moore-Penrose pseudo-inverse of M. * A damped least-squares solver is used to compute M<sup>+</sup>. * @param matrixToComputeNullspaceOf the matrix to compute the nullspace of for the projection. Not Modified. * @param nullspaceToPack matrix to store the resulting nullspace matrix. Modified. */ public void computeNullspace(DenseMatrix64F matrixToComputeNullspaceOf, DenseMatrix64F nullspaceToPack) { int nullspaceMatrixSize = matrixToComputeNullspaceOf.getNumCols(); nullspaceToPack.reshape(nullspaceMatrixSize, nullspaceMatrixSize); pseudoInverseMatrix.reshape(nullspaceMatrixSize, matrixToComputeNullspaceOf.getNumRows()); pseudoInverseSolver.setA(matrixToComputeNullspaceOf); pseudoInverseSolver.invert(pseudoInverseMatrix); // I - J^* J CommonOps.mult(-1.0, pseudoInverseMatrix, matrixToComputeNullspaceOf, nullspaceToPack); for (int i = 0; i < nullspaceMatrixSize; i++) nullspaceToPack.add(i, i, 1.0); }
private double applyTimeUpdate() { timeUpdate.set(timeGradient); CommonOps.scale(-timeGain.getDoubleValue(), timeUpdate); double maxUpdate = CommonOps.elementMaxAbs(timeUpdate); double minIntervalTime = CommonOps.elementMin(intervalTimes); if (maxUpdate > 0.4 * minIntervalTime) { CommonOps.scale(0.4 * minIntervalTime / maxUpdate, timeUpdate); } for (int i = 0; i < intervals.getIntegerValue(); i++) { intervalTimes.add(i, 0, timeUpdate.get(i)); } return solveMinAcceleration(); }
private double applyTimeUpdate() { timeUpdate.set(timeGradient); CommonOps.scale(-timeGain.getDoubleValue(), timeUpdate); double maxUpdate = CommonOps.elementMaxAbs(timeUpdate); double minIntervalTime = CommonOps.elementMin(intervalTimes); if (maxUpdate > 0.4 * minIntervalTime) { CommonOps.scale(0.4 * minIntervalTime / maxUpdate, timeUpdate); } for (int i = 0; i < intervals.getIntegerValue(); i++) { intervalTimes.add(i, 0, timeUpdate.get(i)); } return solveMinAcceleration(); }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomNumbers.nextDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clamp(correction.get(i, 0), -maxStepSize, maxStepSize)); } }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomTools.generateRandomDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clipToMinMax(correction.get(i, 0), -maxStepSize, maxStepSize)); } }