/** * Creates a linear solver using QR decomposition */ public static LinearSolver<DenseMatrix64F> qr( int numRows , int numCols ) { return leastSquares(numRows,numCols); }
/** * Creates a general purpose solver. Use this if you are not sure what you need. * * @param numRows The number of rows that the decomposition is optimized for. * @param numCols The number of columns that the decomposition is optimized for. */ public static LinearSolver<DenseMatrix64F> general( int numRows , int numCols ) { if( numRows == numCols ) return linear(numRows); else return leastSquares(numRows,numCols); }
/** * Creates a general purpose solver. Use this if you are not sure what you need. * * @param numRows The number of rows that the decomposition is optimized for. * @param numCols The number of columns that the decomposition is optimized for. */ public static LinearSolver<DenseMatrix64F> general( int numRows , int numCols ) { if( numRows == numCols ) return linear(numRows); else return leastSquares(numRows,numCols); }
/** * Creates a general purpose solver. Use this if you are not sure what you need. * * @param numRows The number of rows that the decomposition is optimized for. * @param numCols The number of columns that the decomposition is optimized for. */ public static LinearSolver<DenseMatrix64F> general( int numRows , int numCols ) { if( numRows == numCols ) return linear(numRows); else return leastSquares(numRows,numCols); }
public static InverseJacobianSolver createInverseJacobianSolver(int maximumNumberOfConstraints, int numberOfDoF, boolean useSVD) { LinearSolver<DenseMatrix64F> solver; if (useSVD) { solver = new SolvePseudoInverseSvd(maximumNumberOfConstraints, maximumNumberOfConstraints); } else { solver = LinearSolverFactory.leastSquares(maximumNumberOfConstraints, maximumNumberOfConstraints); } return new InverseJacobianSolver(maximumNumberOfConstraints, numberOfDoF, solver); }
public static InverseJacobianSolver createInverseJacobianSolver(int maximumNumberOfConstraints, int numberOfDoF, boolean useSVD) { LinearSolver<DenseMatrix64F> solver; if (useSVD) { solver = new SolvePseudoInverseSvd(maximumNumberOfConstraints, maximumNumberOfConstraints); } else { solver = LinearSolverFactory.leastSquares(maximumNumberOfConstraints, maximumNumberOfConstraints); } return new InverseJacobianSolver(maximumNumberOfConstraints, numberOfDoF, solver); }
@Override public void process() { DenseMatrix64F a = ((VariableMatrix)A).matrix; DenseMatrix64F b = ((VariableMatrix)B).matrix; if( solver == null ) { solver = LinearSolverFactory.leastSquares(a.numRows,a.numCols); } if( !solver.setA(a)) throw new RuntimeException("Solver failed!"); output.matrix.reshape(a.numCols,b.numCols); solver.solve(b, output.matrix); } };
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double tolerance, int maxIterations, double maxStepSize, double minRandomSearchScalar, double maxRandomSearchScalar) { if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); this.jacobian = jacobian; this.solver = LinearSolverFactory.leastSquares(SpatialMotionVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = ScrewTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double tolerance, int maxIterations, double maxStepSize, double minRandomSearchScalar, double maxRandomSearchScalar) { if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); this.jacobian = jacobian; this.solver = LinearSolverFactory.leastSquares(SpatialVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = MultiBodySystemTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
public PolynomialFittingEncoderProcessor(String name, IntegerYoVariable rawPosition, DoubleYoVariable time, double distancePerTick, int nEncoderEvents, int fitOrder, int skipFactor, YoVariableRegistry registry) { super(name, rawPosition, time, distancePerTick, registry); if (fitOrder > nEncoderEvents - 1) throw new RuntimeException("Cannot fit a polynomial of order " + fitOrder + " through " + nEncoderEvents + " encoder events."); this.nEncoderEvents = nEncoderEvents; this.fitOrder = fitOrder; this.skipFactor = skipFactor; positions = new IntegerYoVariable[nEncoderEvents]; timestamps = new DoubleYoVariable[nEncoderEvents]; for (int i = 0; i < nEncoderEvents; i++) { positions[i] = new IntegerYoVariable(name + "Pos" + i, registry); timestamps[i] = new DoubleYoVariable(name + "Time" + i, registry); } a = new DenseMatrix64F(nEncoderEvents, fitOrder + 1); b = new DenseMatrix64F(a.getNumRows(), 1); p = new DenseMatrix64F(a.getNumCols(), 1); solver = LinearSolverFactory.leastSquares(a.getNumRows(), a.getNumCols()); }
LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.leastSquares(100, 100);
jacobianTimesJaconianTransposedPlusLamdaSquaredMatrix = new DenseMatrix64F(numJoints, numJoints); solver = LinearSolverFactory.leastSquares(SpatialMotionVector.SIZE, SpatialMotionVector.SIZE); translationSelectionMatrix = new DenseMatrix64F(3, SpatialMotionVector.SIZE); translationSelectionMatrix.set(0, 3, 1.0);