public static DenseMatrix64F mult(DenseMatrix64F A, DenseMatrix64F B) { DenseMatrix64F C = new DenseMatrix64F(A.getNumRows(), B.getNumCols()); CommonOps.mult(A, B, C); return C; }
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); } } }
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 setSelectionMatrix(DenseMatrix64F selectionMatrix) { numberOfConstraints = selectionMatrix.getNumRows(); this.selectionMatrix.reshape(numberOfConstraints, selectionMatrix.getNumCols()); this.selectionMatrix.set(selectionMatrix); }
public static void setMatrixColumnToVector(int columnIndex, DenseMatrix64F Matrix, DenseMatrix64F vector) { for (int i = 0; i < Matrix.getNumRows(); i++) { Matrix.set(i, columnIndex, vector.get(i, columnIndex)); } }
private void computeDampedSigmaOperator(DenseMatrix64F dampedSigmaToPack, DenseMatrix64F sigma, double alpha) { int minor = Math.min(sigma.getNumRows(), sigma.getNumCols()); dampedSigmaToPack.reshape(minor, minor); dampedSigmaToPack.zero(); for (int i = 0; i < minor; i++) { double sigmaValue = sigma.get(i, i); double dampedSigma = sigmaValue * sigmaValue / (sigmaValue * sigmaValue + alpha * alpha); dampedSigmaToPack.set(i, i, dampedSigma); } } }
@Override public void setLowerBounds(DenseMatrix64F variableLowerBounds) { int numberOfLowerBounds = variableLowerBounds.getNumRows(); if (numberOfLowerBounds != quadraticCostQMatrix.getNumRows()) throw new RuntimeException("variableLowerBounds.getNumRows() != quadraticCostQMatrix.getNumRows()"); lowerBoundsCMatrix.reshape(numberOfLowerBounds, numberOfLowerBounds); CommonOps.setIdentity(lowerBoundsCMatrix); this.variableLowerBounds.set(variableLowerBounds); CommonOps.scale(-1.0, this.variableLowerBounds); }
private DenseMatrix64F getSymmetricMatrix(int numVariables, DenseMatrix64F dataMatrix, DenseMatrix64F dataMatrixTranspose) { DenseMatrix64F symmetricMatrix = new DenseMatrix64F(numVariables, numVariables); mult(dataMatrixTranspose, dataMatrix, symmetricMatrix); for (int i = 0; i<dataMatrix.getNumCols(); i++) { double diagonalElement = ridgeRegularizationConstant + symmetricMatrix.get(i, i); symmetricMatrix.set(i, i, diagonalElement); } return symmetricMatrix; }
public void setDesiredAcceleration(DenseMatrix64F qdd, int[] indices) { if (indices.length != 6) throw new RuntimeException("Unexpected number of indices: " + Arrays.toString(indices)); desiredAcceleration.reshape(6, 1); for (int i = 0; i < indices.length; i++) desiredAcceleration.set(i, 0, qdd.get(indices[i], 0)); }
private double calculateICPQuantityScalar(DenseMatrix64F generalizedAlphaBetaPrimeMatrix, DenseMatrix64F generalizedGammaPrimeMatrix, DenseMatrix64F polynomialCoefficientVector, double icpPositionDesiredFinal) { DenseMatrix64F M1 = new DenseMatrix64F(generalizedAlphaBetaPrimeMatrix.getNumRows(), polynomialCoefficientVector.getNumCols()); M1.zero(); CommonOps.mult(generalizedAlphaBetaPrimeMatrix, polynomialCoefficientVector, M1); DenseMatrix64F M2 = new DenseMatrix64F(generalizedGammaPrimeMatrix); CommonOps.scale(icpPositionDesiredFinal, M2); CommonOps.addEquals(M1, M2); return M1.get(0, 0); }
private static DenseMatrix64F createCovarianceMatrix(double standardDeviationXY, double standardDeviationZ, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); orientationCovarianceMatrix.set(0, 0, standardDeviationXY * standardDeviationXY); orientationCovarianceMatrix.set(1, 1, standardDeviationXY * standardDeviationXY); orientationCovarianceMatrix.set(2, 2, standardDeviationZ * standardDeviationZ); return orientationCovarianceMatrix; }
@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)); }
private void smartPrintOutADotV(double epsilon) { DenseMatrix64F difference = new DenseMatrix64F(aDotVNumerical.numRows, aDotVNumerical.numCols); CommonOps.subtract(aDotVNumerical, aDotVAnalytical, difference); for (int i = 0; i < difference.numRows; i++) if(Math.abs(difference.get(i, 0)) > epsilon) { printOutADotV(); break; } }
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 boolean isHardConstraint() { for (int i = 0; i < weightVector.getNumRows(); i++) { if (weightVector.get(i, 0) == HARD_CONSTRAINT) return true; } return false; }
/** * Creates a decompose that defines the specified amount of memory. * * @param numElements number of elements in the matrix. */ public BidiagonalDecompositionRow_D64(int numElements) { UBV = new DenseMatrix64F(numElements); gammasU = new double[ numElements ]; gammasV = new double[ numElements ]; b = new double[ numElements ]; u = new double[ numElements ]; }
public void setLinearMomentumRateOfChange(FrameVector linearMomentumRateOfChange) { selectionMatrix.reshape(3, SpatialMotionVector.SIZE); selectionMatrix.set(0, 3, 1.0); selectionMatrix.set(1, 4, 1.0); selectionMatrix.set(2, 5, 1.0); momentumRate.reshape(selectionMatrix.getNumCols(), 1); MatrixTools.setDenseMatrixFromTuple3d(momentumRate, linearMomentumRateOfChange.getVector(), 3, 0); }
public static void maximize(DenseMatrix64F p, double minval) { int rows = p.getNumRows(); int cols = p.getNumCols(); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { double val = p.get(i, j); if(val<minval) p.unsafe_set(j, j, minval); } } }