private void computeDynamicConstraint() { addDynamicRelaxationToDynamicConstraint(); if (useFeedback) addFeedbackToDynamicConstraint(); if (useStepAdjustment) addFootstepRecursionsToDynamicConstraint(); CommonOps.subtractEquals(currentICP, finalICPRecursion); CommonOps.subtractEquals(currentICP, stanceCMPProjection); CommonOps.subtractEquals(currentICP, initialICPProjection); if (useTwoCMPs) CommonOps.subtractEquals(currentICP, cmpOffsetRecursionEffect); MatrixTools.setMatrixBlock(dynamics_beq, 0, 0, currentICP, 0, 0, 2, 1, 1.0); }
private void updateAPosterioriStateCovariance() { // P = (I-KH)P = P - (KH)P = P-K(HP) MatrixMatrixMult.mult_small(H, P, c); MatrixMatrixMult.mult_small(K, c, b); CommonOps.subtractEquals(P, b); }
private void updateAPosterioriStateCovariance() { // P = (I-KH)P = P - (KH)P = P-K(HP) MatrixMatrixMult.mult_small(H, P, c); MatrixMatrixMult.mult_small(K, c, b); CommonOps.subtractEquals(P, b); }
public static void numericallyDifferentiate(DenseMatrix64F derivativeToPack, DenseMatrix64F previousMatrixToUpdate, DenseMatrix64F newMatrix, double dt) { derivativeToPack.set(newMatrix); CommonOps.subtractEquals(derivativeToPack, previousMatrixToUpdate); CommonOps.scale(1.0 / dt, derivativeToPack); previousMatrixToUpdate.set(newMatrix); }
public static void numericallyDifferentiate(DenseMatrix64F derivativeToPack, DenseMatrix64F previousMatrixToUpdate, DenseMatrix64F newMatrix, double dt) { derivativeToPack.set(newMatrix); CommonOps.subtractEquals(derivativeToPack, previousMatrixToUpdate); CommonOps.scale(1.0 / dt, derivativeToPack); previousMatrixToUpdate.set(newMatrix); }
public DenseMatrix64F getMomentumDotEquationRightHandSide(MomentumRateCommand momentumRateCommand) { DenseMatrix64F selectionMatrix = momentumRateCommand.getSelectionMatrix(); DenseMatrix64F momentumRate = momentumRateCommand.getMomentumRate(); CommonOps.mult(selectionMatrix, momentumRate, centroidalMomentumEquationRightHandSide); CommonOps.subtractEquals(centroidalMomentumEquationRightHandSide, adotV); return centroidalMomentumEquationRightHandSide; } }
private double getMaxInequalityConstraintError(int numberOfInequalityConstraints, DenseMatrix64F linearInequalityConstraintsCMatrix, DenseMatrix64F linearInequalityConstraintsDVector, DenseMatrix64F solutionMatrix) { DenseMatrix64F checkMatrix = new DenseMatrix64F(numberOfInequalityConstraints, 1); CommonOps.mult(linearInequalityConstraintsCMatrix, solutionMatrix, checkMatrix); CommonOps.subtractEquals(checkMatrix, linearInequalityConstraintsDVector); return getMaxSignedDataEntry(checkMatrix); }
private double getMaxEqualityConstraintError(int numberOfEqualityConstraints, DenseMatrix64F linearEqualityConstraintsAMatrix, DenseMatrix64F linearEqualityConstraintsBVector, DenseMatrix64F solutionMatrix) { DenseMatrix64F checkMatrix = new DenseMatrix64F(numberOfEqualityConstraints, 1); CommonOps.mult(linearEqualityConstraintsAMatrix, solutionMatrix, checkMatrix); CommonOps.subtractEquals(checkMatrix, linearEqualityConstraintsBVector); return getMaxAbsoluteDataEntry(checkMatrix); }
private double getMaxEqualityConstraintError(int numberOfEqualityConstraints, DenseMatrix64F linearEqualityConstraintsAMatrix, DenseMatrix64F linearEqualityConstraintsBVector, DenseMatrix64F solutionMatrix) { DenseMatrix64F checkMatrix = new DenseMatrix64F(numberOfEqualityConstraints, 1); CommonOps.mult(linearEqualityConstraintsAMatrix, solutionMatrix, checkMatrix); CommonOps.subtractEquals(checkMatrix, linearEqualityConstraintsBVector); return getMaxAbsoluteDataEntry(checkMatrix); }
private double getMaxEqualityConstraintError(int numberOfEqualityConstraints, DenseMatrix64F linearEqualityConstraintsAMatrix, DenseMatrix64F linearEqualityConstraintsBVector, DenseMatrix64F solutionMatrix) { DenseMatrix64F checkMatrix = new DenseMatrix64F(numberOfEqualityConstraints, 1); CommonOps.mult(linearEqualityConstraintsAMatrix, solutionMatrix, checkMatrix); CommonOps.subtractEquals(checkMatrix, linearEqualityConstraintsBVector); return getMaxAbsoluteDataEntry(checkMatrix); }
private double getMaxInequalityConstraintError(int numberOfInequalityConstraints, DenseMatrix64F linearInequalityConstraintsCMatrix, DenseMatrix64F linearInequalityConstraintsDVector, DenseMatrix64F solutionMatrix) { DenseMatrix64F checkMatrix = new DenseMatrix64F(numberOfInequalityConstraints, 1); CommonOps.mult(linearInequalityConstraintsCMatrix, solutionMatrix, checkMatrix); CommonOps.subtractEquals(checkMatrix, linearInequalityConstraintsDVector); return getMaxSignedDataEntry(checkMatrix); }
/** * Updates the IMU given the rate gyro inputs. * * @param pqr Matrix Gyro Rate values in order of qd_wy, qd_wx, qd_wz??? */ public void imuUpdate(DenseMatrix64F pqr) { CommonOps.subtractEquals(pqr, bias); // unpackQuaternion(q); makeAMatrix(pqr); propagateState(pqr); propagateCovariance(A); }
/** * Updates the IMU given the rate gyro inputs. * * @param pqr Matrix Gyro Rate values in order of qd_wy, qd_wx, qd_wz??? */ public void imuUpdate(DenseMatrix64F pqr) { CommonOps.subtractEquals(pqr, bias); // unpackQuaternion(q); makeAMatrix(pqr); propagateState(pqr); propagateCovariance(A); }
public boolean areRigidBodyDynamicsSatisfied(DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F tau, DenseMatrix64F rho) { computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); CommonOps.multAdd(bodyMassMatrix, qddot, bodyCoriolisMatrix); CommonOps.multAdd(-1.0, matrixTranspose, rho, bodyCoriolisMatrix); CommonOps.subtractEquals(bodyCoriolisMatrix, tau); return equalsZero(bodyCoriolisMatrix, tolerance); }
private void addMotionTaskInternal(DenseMatrix64F taskJtW, DenseMatrix64F taskJacobian, DenseMatrix64F taskObjective) { // Compute: H += J^T W J tempTask_H.reshape(numberOfDoFs, numberOfDoFs); CommonOps.mult(taskJtW, taskJacobian, tempTask_H); CommonOps.addEquals(solverInput_H, tempTask_H); // Compute: f += - J^T W xDot tempTask_f.reshape(numberOfDoFs, 1); CommonOps.mult(taskJtW, taskObjective, tempTask_f); CommonOps.subtractEquals(solverInput_f, tempTask_f); }
public boolean areConstraintsEnforcedSuccesfully(DenseMatrix64F x, DenseMatrix64F j, DenseMatrix64F p, double epsilon) { // This check is only valid if you use an 'exact' solver, not if you're using the damped least squares 'pseudoinverse' if (j.getNumRows() > 0) { check.reshape(p.getNumRows(), 1); CommonOps.mult(j, x, check); CommonOps.subtractEquals(check, p); return MatrixFeatures.isConstantVal(check, 0.0, epsilon); } return true; } }
public boolean areConstraintsEnforcedSuccesfully(DenseMatrix64F x, DenseMatrix64F j, DenseMatrix64F p, double epsilon) { // This check is only valid if you use an 'exact' solver, not if you're using the damped least squares 'pseudoinverse' if (j.getNumRows() > 0) { check.reshape(p.getNumRows(), 1); CommonOps.mult(j, x, check); CommonOps.subtractEquals(check, p); return MatrixFeatures.isConstantVal(check, 0.0, epsilon); } return true; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testUnconstrained() { Random random = new Random(12355L); int objectiveSize = 5; int solutionSize = 5; int constraintSize = 0; QuadraticProgram quadraticProgram = createRandomQuadraticProgram(random, objectiveSize, solutionSize, constraintSize); DenseMatrix64F initialGuess = new DenseMatrix64F(solutionSize, 1); ActiveSearchSolutionInfo solutionInfo = solve(quadraticProgram, initialGuess); assertTrue(solutionInfo.isConverged()); DenseMatrix64F axMinusB = new DenseMatrix64F(solutionSize, 1); CommonOps.mult(quadraticProgram.getA(), solutionInfo.getSolution(), axMinusB); CommonOps.subtractEquals(axMinusB, quadraticProgram.getB()); assertTrue(MatrixFeatures.isConstantVal(axMinusB, 0.0, 1e-12)); }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); if (!jacobianSolver.setA(jacobian.getJacobianMatrix())) throw new RuntimeException("jacobian cannot be solved"); jacobianSolver.solve(biasedSpatialAcceleration, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); jacobianSolver.setJacobian(jacobian.getJacobianMatrix()); jacobianSolver.solve(jointAccelerations, biasedSpatialAcceleration); CommonOps.scale(sign, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }