us.ihmc.robotics.math.frames.YoMatrix类的使用及代码示例

x33g5p2x  于2022-02-05 转载在 其他  
字(11.9k)|赞(0)|评价(0)|浏览(137)

本文整理了Java中us.ihmc.robotics.math.frames.YoMatrix类的一些代码示例，展示了YoMatrix类的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。YoMatrix类的具体详情如下：
包路径：us.ihmc.robotics.math.frames.YoMatrix
类名称：YoMatrix

YoMatrix介绍

[英]YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a maximum number of rows and columns and an actual number of rows and columns. If you set with a smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will be set to NaN. If you get the contents the matrix you pack must be the correct size.
[中]YoMatrix。对象，用于保存变量矩阵，以便矩阵可以被重绕。具有最大行数和列数以及实际行数和列数。如果使用较小的矩阵进行设置，则实际大小将是传入矩阵的大小。额外条目将设置为NaN。如果你得到的内容，你包装的矩阵必须是正确的大小。

代码示例

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void getAndReshape(DenseMatrix64F matrixToPack)
{
 matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns());
 get(matrixToPack);
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit-test

@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);  
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit-test

@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);
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit-test

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);

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void submitMomentumSelectionMatrix(DenseMatrix64F momentumSelectionMatrix)
{
 this.momentumSelectionMatrix.set(momentumSelectionMatrix);
 yoMomentumSelectionMatrix.set(momentumSelectionMatrix);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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);
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit-test

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);

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

desiredCoPCommandInSoleFrame = new YoFramePoint2d(namePrefix + "DesiredCoPCommand", planeFrame, registry);
yoRho = new YoMatrix(namePrefix + "Rho", rhoSize, 1, registry);

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public void getAndReshape(DenseMatrix64F matrixToPack)
{
 matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns());
 get(matrixToPack);
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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);
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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;
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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);

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

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();
 }
}

内容来源于网络，如有侵权，请联系作者删除！

YoMatrix

相关文章

    查看更多

    热门标签

    更多
    JavaquerypythonNode开发语言requestUtil数据库Table后端算法LoggerMessageElementParser

    最新文章

    更多
    • 蜀ICP备13028337号-1 大数据知识库 https://www.saoniuhuo.com © All rights reserved
    • 本站内容来源互联网,如果侵犯您的权益请联系我们删除, 联系方式：448109455@qq.com