public static Matrix identity(int size) { Matrix result = new Matrix(size, size); CommonOps.setIdentity(result.mat); return result; }
private void computeCenterOfMassPositionStateMatrixBlock() { DenseMatrix64F stateMatrixBlock = stateMatrixBlocks.get(centerOfMassPositionPort); CommonOps.setIdentity(stateMatrixBlock); }
public static void invert( LinearSolver<DenseMatrix64F> solver , RowD1Matrix64F A , DenseMatrix64F A_inv , DenseMatrix64F storage) { if( A.numRows != A_inv.numRows || A.numCols != A_inv.numCols) { throw new IllegalArgumentException("A and A_inv must have the same dimensions"); } CommonOps.setIdentity(storage); solver.solve(storage,A_inv); }
public static void invert( LinearSolver<DenseMatrix64F> solver , RowD1Matrix64F A , DenseMatrix64F A_inv , DenseMatrix64F storage) { if( A.numRows != A_inv.numRows || A.numCols != A_inv.numCols) { throw new IllegalArgumentException("A and A_inv must have the same dimensions"); } CommonOps.setIdentity(storage); solver.solve(storage,A_inv); }
public void setFeedbackRegularizationWeight(double regularizationWeight) { CommonOps.setIdentity(feedbackRegularizationWeight); CommonOps.scale(regularizationWeight, feedbackRegularizationWeight); hasFeedbackRegularizationTerm = true; }
public void setFootstepRegularizationWeight(double regularizationWeight) { CommonOps.setIdentity(footstepRegularizationWeight); CommonOps.scale(regularizationWeight, footstepRegularizationWeight); hasFootstepRegularizationTerm = true; }
@Override public void getFMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(size, size); CommonOps.setIdentity(matrixToPack); }
public static void invert( LinearSolver<DenseMatrix64F> solver , RowD1Matrix64F A , DenseMatrix64F A_inv ) { if( A.numRows != A_inv.numRows || A.numCols != A_inv.numCols) { throw new IllegalArgumentException("A and A_inv must have the same dimensions"); } CommonOps.setIdentity(A_inv); solver.solve(A_inv,A_inv); } }
public void setSelectionMatrixForFullConstraint() { numberOfConstraints = maximumNumberOfConstraints; this.selectionMatrix.reshape(numberOfConstraints, numberOfConstraints); CommonOps.setIdentity(selectionMatrix); }
private void computeAngularAccelerationInputMatrixBlock() { DenseMatrix64F angularAccelerationInputBlock = new DenseMatrix64F(SIZE, SIZE); CommonOps.setIdentity(angularAccelerationInputBlock); inputMatrixBlocks.put(angularAccelerationInputPort, angularAccelerationInputBlock); }
private void setSelectionMatrixToIdentity() { selectionMatrix.reshape(Twist.SIZE, Twist.SIZE); CommonOps.setIdentity(selectionMatrix); }
@Override public void process() { int N = ((VariableInteger)A).value; output.matrix.reshape(N,N); CommonOps.setIdentity(output.matrix); } };
@Override public void clear(ReferenceFrame referenceFrame) { super.clear(referenceFrame); commandId = Packet.VALID_MESSAGE_DEFAULT_ID; executionMode = ExecutionMode.OVERRIDE; previousCommandId = Packet.INVALID_MESSAGE_ID; selectionMatrix.reshape(6, 6); CommonOps.setIdentity(selectionMatrix); }
public void set(SpatialForceVector momentum) { selectionMatrix.reshape(SpatialForceVector.SIZE, SpatialForceVector.SIZE); CommonOps.setIdentity(selectionMatrix); this.momentum.reshape(SpatialForceVector.SIZE, 1); momentum.getMatrix(this.momentum); }
public void getWeightMatrix(DenseMatrix64F weightMatrix) { weightMatrix.reshape(SpatialMotionVector.SIZE, SpatialMotionVector.SIZE); CommonOps.setIdentity(weightMatrix); for (int i = 0; i < SpatialMotionVector.SIZE; i++) weightMatrix.set(i, i, weightVector.get(i, 0)); }
public JointState(String jointName, String parameterGroup, double dt, YoVariableRegistry registry) { this.jointName = jointName; this.sqrtHz = 1.0 / Math.sqrt(dt); CommonOps.setIdentity(F); F.set(0, 1, dt); F.set(0, 2, 0.5 * dt * dt); F.set(1, 2, dt); accelerationVariance = FilterTools.findOrCreate(parameterGroup + "AccelerationVariance", registry, 1.0); }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
private static DenseMatrix64F createDiagonalCovarianceMatrix(double standardDeviation, int size) { DenseMatrix64F orientationCovarianceMatrix = new DenseMatrix64F(size, size); CommonOps.setIdentity(orientationCovarianceMatrix); CommonOps.scale(MathTools.square(standardDeviation), orientationCovarianceMatrix); return orientationCovarianceMatrix; }
public void set(SpatialForceVector momentumRateOfChange) { selectionMatrix.reshape(SpatialForceVector.SIZE, SpatialForceVector.SIZE); CommonOps.setIdentity(selectionMatrix); momentumRate.reshape(SpatialForceVector.SIZE, 1); momentumRateOfChange.getMatrix(momentumRate); }
@Override public void setUpperBounds(DenseMatrix64F variableUpperBounds) { int numberOfUpperBounds = variableUpperBounds.getNumRows(); if (numberOfUpperBounds != quadraticCostQMatrix.getNumRows()) throw new RuntimeException("variableUpperBounds.getNumRows() != quadraticCostQMatrix.getNumRows()"); upperBoundsCMatrix.reshape(numberOfUpperBounds, numberOfUpperBounds); CommonOps.setIdentity(upperBoundsCMatrix); CommonOps.scale(-1.0, upperBoundsCMatrix); this.variableUpperBounds.set(variableUpperBounds); }