@Override public void solve(DenseMatrix64F b, DenseMatrix64F x) { CommonOps.mult(pseudoInverse, b, x); }
public void constrainEquation(DenseMatrix64F a, DenseMatrix64F b) { aCopy.set(a); CommonOps.mult(aCopy, q, a); CommonOps.multAdd(-1.0, aCopy, jPlusp, b); }
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); } }
private void computeSubspaceSpatialVelocity(DenseMatrix64F subspaceSpatialVelocityToPack, DenseMatrix64F spatialVelocity) { subspaceSpatialVelocityToPack.reshape(numberOfConstraints, 1); CommonOps.mult(selectionMatrix, spatialVelocity, subspaceSpatialVelocityToPack); }
/** * Returns the algebraic error vector. error = A*U*x. length = number * of observations */ public void computeErrorVector( DenseMatrix64F A , DenseMatrix64F errors ) { errors.reshape(A.numRows,1); CommonOps.mult(A,vectorT,errors); }
private double computeInTransferVelocity() { CommonOps.mult(cubicProjectionDerivativeMatrix, transferExitCMPProjectionMatrix, matrixOut); return matrixOut.get(0, 0); }
@Override public void predict() { tempStateVector.set(stateVector); CommonOps.mult(F, tempStateVector, stateVector); }
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 void update(double timeRemaining) { double timeInCurrentState = duration - timeRemaining; timeInCurrentState = MathTools.clipToMinMax(timeInCurrentState, 0.0, duration); cubicTimeMatrix.setCurrentTime(timeInCurrentState); CommonOps.mult(cubicTimeMatrix, cubicSplineCoefficientMatrix, this); } }
public static double quality( DenseMatrix64F orig , DenseMatrix64F U , DenseMatrix64F W , DenseMatrix64F Vt ) { // foundA = U*W*Vt DenseMatrix64F UW = new DenseMatrix64F(U.numRows,W.numCols); CommonOps.mult(U,W,UW); DenseMatrix64F foundA = new DenseMatrix64F(UW.numRows,Vt.numCols); CommonOps.mult(UW,Vt,foundA); double normA = NormOps.normF(foundA); return SpecializedOps.diffNormF(orig,foundA)/normA; }
public static DenseMatrix64F mult(DenseMatrix64F A, DenseMatrix64F B) { DenseMatrix64F C = new DenseMatrix64F(A.getNumRows(), B.getNumCols()); CommonOps.mult(A, B, C); return C; }
public DenseMatrix64F checkJQEqualsZeroAfterSetConstraint() { DenseMatrix64F checkJQEqualsZero = new DenseMatrix64F(this.j.getNumRows(), q.getNumCols()); CommonOps.mult(this.j, q, checkJQEqualsZero); return checkJQEqualsZero; }
public DenseMatrix64F constrainResult(DenseMatrix64F xBar) { x.reshape(xBar.getNumRows(), 1); CommonOps.mult(q, xBar, x); CommonOps.addEquals(x, jPlusp); return x; }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
public DenseMatrix64F getMomentumDotEquationRightHandSide(MomentumRateCommand momentumRateCommand) { DenseMatrix64F selectionMatrix = momentumRateCommand.getSelectionMatrix(); DenseMatrix64F momentumRate = momentumRateCommand.getMomentumRate(); CommonOps.mult(selectionMatrix, momentumRate, centroidalMomentumEquationRightHandSide); CommonOps.subtractEquals(centroidalMomentumEquationRightHandSide, adotV); return centroidalMomentumEquationRightHandSide; } }
public void update(double sin_x, double sin_y, double cos_x, double cos_y) { this.sx = sin_x; this.sy = sin_y; this.cx = cos_x; this.cy = cos_y; Pmat.set(3,3,true,new double[] {cy, sx*sy, cx*sy, 0, cx, -sx, -sy, sx*cy, cx*cy}); CommonOps.mult(Pmat, P0, P); computePulley(); computeJacobianRow(); }
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); }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); geometricJacobian.changeFrame(geometricJacobianFrame); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
private void minimizeError() { jacobianMethod.set(getUpdatedJacobianMatrix()); spatialError.set(getSpatialErrorEndEffectorDesired()); jacobianOperation(solveMethod); CommonOps.mult(jacobianMethod, spatialError, correction); }