public static Matrix identity(int size) { Matrix result = new Matrix(size, size); CommonOps.setIdentity(result.mat); return result; }
private static DenseMatrix64F computeCost(ICPQPInput icpqpInput, DenseMatrix64F solution) { DenseMatrix64F tempMatrix = new DenseMatrix64F(solution.numRows, 1); DenseMatrix64F cost = new DenseMatrix64F(1, 1); CommonOps.mult(icpqpInput.quadraticTerm, solution, tempMatrix); CommonOps.multTransA(solution, tempMatrix, cost); CommonOps.scale(0.5, cost); CommonOps.multAddTransA(-1.0, icpqpInput.linearTerm, solution, cost); CommonOps.addEquals(cost, icpqpInput.residualCost); return cost; } }
public void computeDirection() { CommonOps.solve( jacobian, errorV, dir ); double norm = NormOps.normP2( dir ); CommonOps.divide( norm, dir ); // compute the directional derivative CommonOps.mult( jacobian, dir, directionalDeriv ); // CommonOps.multTransA( dir, directionalDeriv, descentDirectionMag ); logger.debug( "descentDirectionMag: " + descentDirectionMag.get( 0 ) ); }
@Override public void solve(DenseMatrix64F b, DenseMatrix64F x) { CommonOps.mult(pseudoInverse, b, x); }
public void setFeedbackRegularizationWeight(double regularizationWeight) { CommonOps.setIdentity(feedbackRegularizationWeight); CommonOps.scale(regularizationWeight, feedbackRegularizationWeight); hasFeedbackRegularizationTerm = true; }
public void setQuadraticCostFunction(DenseMatrix64F costQuadraticMatrix, DenseMatrix64F costLinearVector, double quadraticCostScalar) { setAndAssertCorrectNumberOfVariablesToSolve(costQuadraticMatrix.numCols); setAndAssertCorrectNumberOfVariablesToSolve(costQuadraticMatrix.numRows); setAndAssertCorrectNumberOfVariablesToSolve(costLinearVector.numRows); DenseMatrix64F symCostQuadraticMatrix = new DenseMatrix64F(costQuadraticMatrix); CommonOps.transpose(symCostQuadraticMatrix); CommonOps.add(costQuadraticMatrix, symCostQuadraticMatrix, symCostQuadraticMatrix); CommonOps.scale(0.5, symCostQuadraticMatrix); this.quadraticCostGMatrix = symCostQuadraticMatrix; this.quadraticCostFVector = costLinearVector; this.quadraticCostScalar = quadraticCostScalar; }
protected void addFeedbackRegularizationTask() { MatrixTools.setMatrixBlock(feedbackRegularizationCost_H, 0, 0, feedbackRegularizationWeight, 0, 0, 2, 2, 1.0); tmpObjective.zero(); tmpObjective.set(previousFeedbackDeltaSolution); CommonOps.mult(feedbackRegularizationWeight, tmpObjective, tmpObjective); CommonOps.multTransA(previousFeedbackDeltaSolution, tmpObjective, feedbackRegularizationResidualCost); MatrixTools.setMatrixBlock(feedbackRegularizationCost_h, 0, 0, tmpObjective, 0, 0, 2, 1, 1.0); CommonOps.addEquals(solverInputResidualCost, feedbackRegularizationResidualCost); MatrixTools.addMatrixBlock(solverInput_H, numberOfFootstepVariables, numberOfFootstepVariables, feedbackRegularizationCost_H, 0, 0, feedbackCMPIndex, feedbackCMPIndex, 1.0); MatrixTools.addMatrixBlock(solverInput_h, numberOfFootstepVariables, 1, feedbackRegularizationCost_h, 0, 0, feedbackCMPIndex, 1, 1.0); }
@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)); }
protected void multQuad(DenseMatrix64F xVector, DenseMatrix64F QMatrix, DenseMatrix64F xTransposeQx) { temporaryMatrix.reshape(xVector.numCols, QMatrix.numCols); CommonOps.multTransA(xVector, QMatrix, temporaryMatrix); CommonOps.mult(temporaryMatrix, xVector, xTransposeQx); } }
public void setMaxRho(DenseMatrix64F rhoMax) { CommonOps.insert(rhoMax, solverInput_ub, numberOfDoFs, 0); } }
@SuppressWarnings("unused") private void timeDerivative(DenseMatrix64F spatialVectorRateToPack, DenseMatrix64F spatialVector, DenseMatrix64F previousSpatialVector) { CommonOps.subtract(spatialVector, previousSpatialVector, spatialVectorRateToPack); CommonOps.scale(1.0 / controlDT, spatialVectorRateToPack); }
/** * Inserts matrix 'src' into matrix 'dest' with the (0,0) of src at (row,col) in dest. * This is equivalent to calling extract(src,0,src.numRows,0,src.numCols,dest,destY0,destX0). * * @param src matrix that is being copied into dest. Not modified. * @param dest Where src is being copied into. Modified. * @param destY0 Start row for the copy into dest. * @param destX0 Start column for the copy into dest. */ public static void insert( ReshapeMatrix64F src, ReshapeMatrix64F dest, int destY0, int destX0) { extract(src,0,src.numRows,0,src.numCols,dest,destY0,destX0); }
@Override public int solve(DenseMatrix64F Q, DenseMatrix64F f, DenseMatrix64F Aeq, DenseMatrix64F beq, DenseMatrix64F Ain, DenseMatrix64F bin, DenseMatrix64F lb, DenseMatrix64F ub, DenseMatrix64F x, boolean initialize) throws NoConvergenceException { allocateTempraryMatrixOnDemand(Q.numCols, Aeq.numRows, Ain.numRows); CommonOps.transpose(Aeq, this.negAeq); CommonOps.scale(-1, this.negAeq); CommonOps.transpose(Ain, this.negAin); CommonOps.scale(-1, this.negAin); return qpWrapper.solve(Q, f, negAeq, beq, negAin, bin, x, initialize); }
private void computeJacobianTransposed(DenseMatrix64F jacobianTransposedToPack, DenseMatrix64F jacobian) { jacobianTransposedToPack.reshape(numberOfDoF, numberOfConstraints); CommonOps.transpose(jacobian, jacobianTransposedToPack); }
@Override public void process() { VariableMatrix mA = (VariableMatrix)A; VariableMatrix mB = (VariableMatrix)B; resize(output, mA.matrix.numRows, mA.matrix.numCols); CommonOps.subtract(mA.matrix, mB.matrix, output.matrix); } };