@Test
public void removeRowFromA() {
int remove = 2;
int m = 5;
int n = 3;
DenseMatrix64F A = RandomMatrices.createRandom(m, n, rand);
// create the modified A
DenseMatrix64F A_e = RandomMatrices.createRandom(m - 1, n, rand);
SubmatrixOps.setSubMatrix(A, A_e, 0, 0, 0, 0, remove, n);
SubmatrixOps.setSubMatrix(A, A_e, remove + 1, 0, remove, 0, m - remove - 1, n);
// Compute the solution to the modified system
DenseMatrix64F X = RandomMatrices.createRandom(n, 2, rand);
DenseMatrix64F Y = new DenseMatrix64F(A_e.numRows, X.numCols);
CommonOps.mult(A_e, X, Y);
// create the solver from the original system then modify it
AdjustableLinearSolver adjSolver = new AdjLinearSolverQr();
adjSolver.setA(A);
adjSolver.removeRowFromA(remove);
// see if it produces the epected results
// solve the system and see if it gets the expected solution
DenseMatrix64F X_found = RandomMatrices.createRandom(X.numRows, X.numCols, rand);
adjSolver.solve(Y, X_found);
// see if they produce the same results
assertTrue(MatrixFeatures.isIdentical(X_found, X, 1e-8));
}
/**
* See if passing in a matrix or not providing one to getQ and getR functions has the same result
*/
@Test
public void checkGetNullVersusNot() {
int width = 5;
int height = 10;
QRDecomposition<DenseMatrix64F> alg = createQRDecomposition();
SimpleMatrix A = new SimpleMatrix(height, width);
RandomMatrices.setRandom(A.getMatrix(), rand);
alg.decompose(A.getMatrix());
// get the results from a provided matrix
DenseMatrix64F Q_provided = RandomMatrices.createRandom(height, height, rand);
DenseMatrix64F R_provided = RandomMatrices.createRandom(height, width, rand);
assertTrue(R_provided == alg.getR(R_provided, false));
assertTrue(Q_provided == alg.getQ(Q_provided, false));
// get the results when no matrix is provided
DenseMatrix64F Q_null = alg.getQ(null, false);
DenseMatrix64F R_null = alg.getR(null, false);
// see if they are the same
assertTrue(MatrixFeatures.isEquals(Q_provided, Q_null));
assertTrue(MatrixFeatures.isEquals(R_provided, R_null));
}
@Test
public void addRowToA() {
int insert = 2;
int m = 5;
int n = 3;
DenseMatrix64F A = RandomMatrices.createRandom(m, n, rand);
double row[] = new double[] {1, 2, 3};
// create the modified A
DenseMatrix64F A_e = RandomMatrices.createRandom(m + 1, n, rand);
SubmatrixOps.setSubMatrix(A, A_e, 0, 0, 0, 0, insert, n);
System.arraycopy(row, 0, A_e.data, insert * n, n);
SubmatrixOps.setSubMatrix(A, A_e, insert, 0, insert + 1, 0, m - insert, n);
// Compute the solution to the modified system
DenseMatrix64F X = RandomMatrices.createRandom(n, 2, rand);
DenseMatrix64F Y = new DenseMatrix64F(A_e.numRows, X.numCols);
CommonOps.mult(A_e, X, Y);
// create the solver from A then add a A. The solver
// should be equivalent to one created from A_e
AdjustableLinearSolver adjSolver = new AdjLinearSolverQr();
assertTrue(adjSolver.setA(A));
adjSolver.addRowToA(row, insert);
// solve the system and see if it gets the expected solution
DenseMatrix64F X_found = RandomMatrices.createRandom(X.numRows, X.numCols, rand);
adjSolver.solve(Y, X_found);
// see if they produce the same results
assertTrue(MatrixFeatures.isIdentical(X_found, X, 1e-8));
}
private static QuadraticProgram createRandomQuadraticProgram(
Random random, int objectiveSize, int solutionSize, int constraintSize) {
DenseMatrix64F a = RandomMatrices.createRandom(objectiveSize, solutionSize, random);
DenseMatrix64F b = RandomMatrices.createRandom(objectiveSize, 1, random);
DenseMatrix64F c = RandomMatrices.createRandom(constraintSize, solutionSize, random);
DenseMatrix64F d = RandomMatrices.createRandom(constraintSize, 1, random);
return new QuadraticProgram(a, b, c, d);
}
@DeployableTestMethod(estimatedDuration = 0.3)
@Test(timeout = 30000)
public void test() {
Random random = new Random(176L);
int matrixSize = 5;
YoVariableRegistry parentRegistry = new YoVariableRegistry("test");
DenseMatrix64F jacobianMatrix = RandomMatrices.createRandom(matrixSize, matrixSize, random);
DenseMatrix64F taskVelocity = RandomMatrices.createRandom(matrixSize, 1, random);
DampedLeastSquaresJacobianSolver solver =
new DampedLeastSquaresJacobianSolver("testSolver", matrixSize, parentRegistry);
solver.setAlpha(1.0);
DenseMatrix64F jointVelocities = new DenseMatrix64F(matrixSize, 1);
solver.setJacobian(jacobianMatrix);
solver.solve(jointVelocities, taskVelocity);
DenseMatrix64F taskVelocityBack = new DenseMatrix64F(matrixSize, 1);
solver.inverseSolve(taskVelocityBack, jointVelocities);
double delta = 1e-9;
JUnitTools.assertMatrixEquals(taskVelocity, taskVelocityBack, delta);
}
/**
* Compare the determinant computed from LU to the value computed from the minor matrix method.
*/
@Test
public void testDeterminant() {
Random rand = new Random(0xfff);
int width = 10;
DenseMatrix64F A = RandomMatrices.createRandom(width, width, rand);
DeterminantFromMinor minor = new DeterminantFromMinor(width);
double minorVal = minor.compute(A);
LUDecompositionAlt alg = new LUDecompositionAlt();
alg.decompose(A);
double luVal = alg.computeDeterminant();
assertEquals(minorVal, luVal, 1e-6);
}
@Test
public void _solveVectorInternal() {
int width = 10;
DenseMatrix64F LU = RandomMatrices.createRandom(width, width, rand);
double a[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
double b[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
for (int i = 0; i < width; i++) LU.set(i, i, 1);
TriangularSolver.solveL(LU.data, a, width);
TriangularSolver.solveU(LU.data, a, width);
DebugDecompose alg = new DebugDecompose(width);
for (int i = 0; i < width; i++) alg.getIndx()[i] = i;
alg.setLU(LU);
alg._solveVectorInternal(b);
for (int i = 0; i < width; i++) {
assertEquals(a[i], b[i], 1e-6);
}
}
