public void getAndReshape(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns()); get(matrixToPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testYoMatrixSetToZero() { int maxNumberOfRows = 4; int maxNumberOfColumns = 8; String name = "testMatrixForZero"; YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); Random random = new Random(1984L); DenseMatrix64F randomMatrix = RandomGeometry.nextDenseMatrix64F(random, maxNumberOfRows, maxNumberOfColumns); yoMatrix.set(randomMatrix); int numberOfRows = 2; int numberOfColumns = 6; yoMatrix.setToZero(numberOfRows, numberOfColumns); DenseMatrix64F zeroMatrix = new DenseMatrix64F(numberOfRows, numberOfColumns); checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, zeroMatrix, registry); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleYoMatrixExample() { int maxNumberOfRows = 4; int maxNumberOfColumns = 8; YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); YoMatrix yoMatrix = new YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry); assertEquals(maxNumberOfRows, yoMatrix.getNumberOfRows()); assertEquals(maxNumberOfColumns, yoMatrix.getNumberOfColumns()); DenseMatrix64F denseMatrix = new DenseMatrix64F(maxNumberOfRows, maxNumberOfColumns); yoMatrix.get(denseMatrix); JUnitTools.assertMatrixEqualsZero(denseMatrix, 1e-10); Random random = new Random(1984L); DenseMatrix64F randomMatrix = RandomGeometry.nextDenseMatrix64F(random, maxNumberOfRows, maxNumberOfColumns); yoMatrix.set(randomMatrix); DenseMatrix64F checkMatrix = new DenseMatrix64F(maxNumberOfRows, maxNumberOfColumns); yoMatrix.get(checkMatrix); JUnitTools.assertMatrixEquals(randomMatrix, checkMatrix, 1e-10); assertEquals(registry.getVariable("testMatrix_0_0").getValueAsDouble(), checkMatrix.get(0, 0), 1e-10); }
String name = "testMatrix"; YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); yoMatrix.set(tooBigMatrix); fail("Too Big"); yoMatrix.set(tooBigMatrix); fail("Too Big"); yoMatrix.set(okMatrix); assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); yoMatrix.getAndReshape(checkMatrix); yoMatrix.set(okMatrix); assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); yoMatrix.getAndReshape(checkMatrix);
public void submitMomentumSelectionMatrix(DenseMatrix64F momentumSelectionMatrix) { this.momentumSelectionMatrix.set(momentumSelectionMatrix); yoMomentumSelectionMatrix.set(momentumSelectionMatrix); }
public void setInputs(DenseMatrix64F a, DenseMatrix64F b, DenseMatrix64F momentumDotWeight, DenseMatrix64F jSecondary, DenseMatrix64F pSecondary, DenseMatrix64F weightMatrixSecondary, DenseMatrix64F WRho, DenseMatrix64F Lambda, DenseMatrix64F WRhoSmoother, DenseMatrix64F rhoPrevAvg, DenseMatrix64F WRhoCop, DenseMatrix64F QRho, DenseMatrix64F c, DenseMatrix64F rhoMin) { //A,b,c momentumOptimizerWithGRFPenalizedSmootherNativeInput.setCentroidalMomentumMatrix(a); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setMomentumDotEquationRightHandSide(b); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setMomentumDotWeight(momentumDotWeight); //Js ps Ws momentumOptimizerWithGRFPenalizedSmootherNativeInput.setSecondaryConstraintJacobian(jSecondary); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setSecondaryConstraintRightHandSide(pSecondary); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setSecondaryConstraintWeight(weightMatrixSecondary); //Wrho, Lambda momentumOptimizerWithGRFPenalizedSmootherNativeInput.setGroundReactionForceRegularization(WRho); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setJointAccelerationRegularization(Lambda); //WRho, Rho_pavg , WRhoCop momentumOptimizerWithGRFPenalizedSmootherNativeInput.setRateOfChangeOfGroundReactionForceRegularization(WRhoSmoother); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setRhoPreviousAverage(rhoPrevAvg); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setCenterOfPressurePenalizedRegularization(WRhoCop); //QRho,c momentumOptimizerWithGRFPenalizedSmootherNativeInput.setContactPointWrenchMatrix(QRho); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setWrenchEquationRightHandSide(c); //Rho_min momentumOptimizerWithGRFPenalizedSmootherNativeInput.setRhoMin(rhoMin); //Wrho rhoPreviousYoMatrix.get(rhoPrevious); momentumOptimizerWithGRFPenalizedSmootherNativeInput.setRhoPrevious(rhoPrevious); }
public CVXMomentumOptimizerAdapter(int nDoF, YoVariableRegistry registry) { rhoSize = CVXMomentumOptimizerWithGRFPenalizedSmootherNative.rhoSize; nSupportVectors = CVXMomentumOptimizerWithGRFPenalizedSmootherNative.nSupportVectors; nPointsPerPlane = CVXMomentumOptimizerWithGRFPenalizedSmootherNative.nPointsPerPlane; nPlanes = CVXMomentumOptimizerWithGRFPenalizedSmootherNative.nPlanes; momentumOptimizerWithGRFPenalizedSmootherNative = new CVXMomentumOptimizerWithGRFPenalizedSmootherNative(nDoF, rhoSize); momentumOptimizerWithGRFPenalizedSmootherNativeInput = new CVXMomentumOptimizerWithGRFPenalizedSmootherNativeInput(); outputRho = new DenseMatrix64F(rhoSize, 1); rhoPrevious = new DenseMatrix64F(rhoSize, 1); rhoPreviousYoMatrix = new YoMatrix("rhoPreviousYoMatrix", rhoSize, 1, registry); }
YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); yoMatrix.get(denseMatrix); fail("Should throw an exception if the size isn't right!"); yoMatrix.getAndReshape(denseMatrix); JUnitTools.assertMatrixEqualsZero(denseMatrix, 1e-10); assertEquals(maxNumberOfRows, denseMatrix.getNumRows()); assertEquals(maxNumberOfColumns, denseMatrix.getNumCols()); assertEquals(maxNumberOfRows, yoMatrix.getNumberOfRows()); assertEquals(maxNumberOfColumns, yoMatrix.getNumberOfColumns()); yoMatrix.set(randomMatrix); yoMatrix.get(checkMatrix); yoMatrix.set(smallerMatrix); assertEquals(smallerRows, yoMatrix.getNumberOfRows()); assertEquals(smallerColumns, yoMatrix.getNumberOfColumns()); yoMatrix.getAndReshape(checkMatrix2);
private void solve(DenseMatrix64F solutionToPack) throws NoConvergenceException { CommonOps.scale(-1.0, solverInput_h); activeSetSolver.clear(); if (localDebug) { yoWeightG.set(solverInput_H); yoWeightg.set(solverInput_h); yoDynamics_Aeq.set(dynamics_Aeq); yoDynamics_beq.set(dynamics_beq); yoSolver_Aeq.set(solverInput_Aeq); yoSolver_beq.set(solverInput_beq); yoStanceCMP_Aeq.set(stanceCMP_Aeq); yoStanceCMP_beq.set(stanceCMP_beq); yoStanceCMPDynamics_Aeq.set(stanceCMPDynamics_Aeq); yoStanceCMPDynamics_beq.set(stanceCMPDynamics_beq); yoStanceCMPSum_Aeq.set(stanceCMPSum_Aeq); yoStanceCMPSum_beq.set(stanceCMPSum_beq); } activeSetSolver.setQuadraticCostFunction(solverInput_H, solverInput_h, 0.0); activeSetSolver.setLinearEqualityConstraints(solverInput_AeqTrans, solverInput_beq); activeSetSolver.setLinearInequalityConstraints(solverInput_AineqTrans, solverInput_bineq); numberOfIterations = activeSetSolver.solve(solutionToPack); if (MatrixTools.containsNaN(solutionToPack)) throw new NoConvergenceException(numberOfIterations); }
desiredCoPCommandInSoleFrame = new YoFramePoint2d(namePrefix + "DesiredCoPCommand", planeFrame, registry); yoRho = new YoMatrix(namePrefix + "Rho", rhoSize, 1, registry);
public void getAndReshape(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns()); get(matrixToPack); }
protected void addStepAdjustmentTask() { footstepObjectiveVector.zero(); for (int i = 0; i < numberOfFootstepsToConsider; i++) { MatrixTools.setMatrixBlock(footstepCost_H, 2 * i, 2 * i, footstepWeights.get(i), 0, 0, 2, 2, 1.0); tmpFootstepObjective.zero(); tmpFootstepObjective.set(referenceFootstepLocations.get(i)); CommonOps.mult(footstepWeights.get(i), tmpFootstepObjective, tmpFootstepObjective); CommonOps.multTransA(referenceFootstepLocations.get(i), tmpFootstepObjective, footstepRegularizationResidualCost); MatrixTools.setMatrixBlock(footstepCost_h, 2 * i, 0, tmpFootstepObjective, 0, 0, 2, 1, 1.0); CommonOps.addEquals(solverInputResidualCost, footstepRegularizationResidualCost); MatrixTools.setMatrixBlock(footstepObjectiveVector, 2 * i, 0, referenceFootstepLocations.get(i), 0, 0, 2, 1, 1.0); } MatrixTools.addMatrixBlock(solverInput_H, 0, 0, footstepCost_H, 0, 0, numberOfFootstepVariables, numberOfFootstepVariables, 1.0); MatrixTools.addMatrixBlock(solverInput_h, 0, 0, footstepCost_h, 0, 0, numberOfFootstepVariables, 1, 1.0); if (localDebug) { footstepReferenceLocation.set(footstepObjectiveVector); footstepH.set(footstepCost_H); footsteph.set(footstepCost_h); } }
public int solve() throws NoConvergenceException { CVXMomentumOptimizerWithGRFPenalizedSmootherNativeOutput momentumOptimizerWithGRFPenalizedSmootherNativeOutput; int ret = -999; try { ret = momentumOptimizerWithGRFPenalizedSmootherNative.solve(momentumOptimizerWithGRFPenalizedSmootherNativeInput); } finally { momentumOptimizerWithGRFPenalizedSmootherNativeOutput = momentumOptimizerWithGRFPenalizedSmootherNative.getOutput(); outputRho = momentumOptimizerWithGRFPenalizedSmootherNativeOutput.getRho(); outputJointAccelerations = momentumOptimizerWithGRFPenalizedSmootherNativeOutput.getJointAccelerations(); outputOptVal = momentumOptimizerWithGRFPenalizedSmootherNativeOutput.getOptVal(); rhoPreviousYoMatrix.set(outputRho); } if (ret < 0) throw new NoConvergenceException(); return ret; }
CommonOps.mult(momentumSelectionMatrix, tempTaskWeight, tempTaskWeightSubspace); CommonOps.multTransB(tempTaskWeightSubspace, momentumSelectionMatrix, momentumWeight); yoMomentumWeight.set(momentumWeight); CommonOps.subtract(momentumRate, additionalWrench, fullMomentumObjective); CommonOps.mult(momentumSelectionMatrix, fullMomentumObjective, momentumObjective); yoMomentumObjective.set(momentumObjective);
public void computeWrenchFromRho(int startIndex, DenseMatrix64F allRobotRho) { CommonOps.extract(allRobotRho, startIndex, startIndex + rhoSize, 0, 1, rhoMatrix, 0, 0); yoRho.set(rhoMatrix); if (yoPlaneContactState.inContact()) { totalWrenchMatrix.zero(); for (int rhoIndex = 0; rhoIndex < rhoSize; rhoIndex++) { double rho = rhoMatrix.get(rhoIndex, 0); CommonOps.extract(rhoJacobianMatrix, 0, SpatialForceVector.SIZE, rhoIndex, rhoIndex + 1, singleRhoWrenchMatrix, 0, 0); MatrixTools.addMatrixBlock(totalWrenchMatrix, 0, 0, singleRhoWrenchMatrix, 0, 0, SpatialForceVector.SIZE, 1, rho); } RigidBody rigidBody = yoPlaneContactState.getRigidBody(); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchFromRho.set(bodyFixedFrame, centerOfMassFrame, totalWrenchMatrix); CommonOps.mult(copJacobianMatrix, rhoMatrix, previousCoPMatrix); previousCoP.setX(previousCoPMatrix.get(0, 0)); previousCoP.setY(previousCoPMatrix.get(1, 0)); } else { wrenchFromRho.setToZero(); } }