/** * @return the number of columns in the Jacobian matrix */ public int getNumberOfColumns() { return jacobian.getNumCols(); }
public void setSelectionMatrix(DenseMatrix64F selectionMatrix) { if (selectionMatrix.getNumCols() != SpatialVector.SIZE) throw new RuntimeException("The selection matrix must have 6 columns, the argument has: " + selectionMatrix.getNumCols()); inverseJacobianCalculator.setSelectionMatrix(selectionMatrix); }
public static void checkVectorDimensions(DenseMatrix64F A, DenseMatrix64F B) { if (A.getNumRows() != B.getNumRows()) { throw new RuntimeException("Got states of different sizes."); } if (A.getNumCols() != 1 || B.getNumCols() != 1) { throw new RuntimeException("States are expected to be row vectors."); } }
@Override public void setLinearEqualityConstraints(DenseMatrix64F linearEqualityConstraintsAMatrix, DenseMatrix64F linearEqualityConstraintsBVector) { if (linearEqualityConstraintsBVector.getNumCols() != 1) throw new RuntimeException("linearEqualityConstraintsBVector.getNumCols() != 1"); if (linearEqualityConstraintsAMatrix.getNumRows() != linearEqualityConstraintsBVector.getNumRows()) throw new RuntimeException("linearEqualityConstraintsAMatrix.getNumRows() != linearEqualityConstraintsBVector.getNumRows()"); if (linearEqualityConstraintsAMatrix.getNumCols() != quadraticCostQMatrix.getNumCols()) throw new RuntimeException("linearEqualityConstraintsAMatrix.getNumCols() != quadraticCostQMatrix.getNumCols()"); this.linearEqualityConstraintsBVector.set(linearEqualityConstraintsBVector); this.linearEqualityConstraintsAMatrix.set(linearEqualityConstraintsAMatrix); }
public static void extractDiagonal(DenseMatrix64F matrix, double[] diagonal) { for (int i = 0; i < Math.min(matrix.getNumRows(), matrix.getNumCols()); i++) { diagonal[i] = matrix.get(i, i); } }
public void setDesiredConfiguration(DenseMatrix64F q) { if (q.getNumRows() != 7 || q.getNumCols() != 1) throw new RuntimeException("Unexpected size: " + q); desiredConfiguration.set(q); }
public void setDesiredAcceleration(DenseMatrix64F qdd) { if (qdd.getNumRows() != 6 || qdd.getNumCols() != 1) throw new RuntimeException("Unexpected size: " + qdd); desiredAcceleration.set(qdd); }
public static void getFromYoVariablesMatrix(DenseMatrix64F m, List<List<YoDouble>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<YoDouble> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, row.get(j).getDoubleValue()); } } }
public static void storeInYoVariablesMatrix(DenseMatrix64F m, List<List<DoubleYoVariable>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<DoubleYoVariable> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { row.get(j).set(m.get(i, j)); } } }
public static void getFromYoVariablesMatrix(DenseMatrix64F m, List<List<DoubleYoVariable>> yoM) { for (int i = 0; i < m.getNumRows(); i++) { List<DoubleYoVariable> row = yoM.get(i); for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, row.get(j).getDoubleValue()); } } }
void zero(DenseMatrix64F a) { for (int i = 0; i < a.getNumRows(); i++) { for (int j = 0; j < a.getNumCols(); j++) { a.set(i, j, 0.0); } } }
private double[][] getXtXInverse(MatrixFormulation matrixFormulation) { DenseMatrix64F XtXInvMatrix = matrixFormulation.XtXInv.copy(); int dim = XtXInvMatrix.getNumCols(); double[][] XtXInvArray = new double[dim][dim]; for (int i = 0; i < dim; i++) { for (int j = 0; j < dim; j++) { XtXInvArray[i][j] = XtXInvMatrix.get(i, j); } } return XtXInvArray; }
public static void setMatrixRowToVector(int rowIndex, DenseMatrix64F Matrix, DenseMatrix64F vector) { for (int i = 0; i < Matrix.getNumCols(); i++) { Matrix.set(rowIndex, i, vector.get(i)); } }
public static DenseMatrix64F mult(DenseMatrix64F A, DenseMatrix64F B) { DenseMatrix64F C = new DenseMatrix64F(A.getNumRows(), B.getNumCols()); CommonOps.mult(A, B, C); return C; }
public DenseMatrix64F checkJQEqualsZeroAfterSetConstraint() { DenseMatrix64F checkJQEqualsZero = new DenseMatrix64F(this.j.getNumRows(), q.getNumCols()); CommonOps.mult(this.j, q, checkJQEqualsZero); return checkJQEqualsZero; }
public static void getFromYoVariables(DenseMatrix64F m, YoDouble[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, yoM[i][j].getDoubleValue()); } } }
public void setSelectionMatrix(DenseMatrix64F selectionMatrix) { numberOfConstraints = selectionMatrix.getNumRows(); this.selectionMatrix.reshape(numberOfConstraints, selectionMatrix.getNumCols()); this.selectionMatrix.set(selectionMatrix); }
public static void getFromYoVariables(DenseMatrix64F m, DoubleYoVariable[][] yoM) { for (int i = 0; i < m.getNumRows(); i++) { for (int j = 0; j < m.getNumCols(); j++) { m.set(i, j, yoM[i][j].getDoubleValue()); } } }
public static void assertMatrixEqualsZero(String message, DenseMatrix64F matrix, double epsilon) { int numberOfRows = matrix.getNumRows(); int numberOfColumns = matrix.getNumCols(); for (int row=0; row<numberOfRows; row++) { for (int column=0; column<numberOfColumns; column++) { assertEquals(message, 0.0, matrix.get(row, column), epsilon); } } }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { MathTools.checkIfInRange(matrix.getNumRows(), 1, Integer.MAX_VALUE); MathTools.checkIfInRange(matrix.getNumCols(), 1, Integer.MAX_VALUE); matrix.set(rowStart, 0, qd); }