public void setSelectionMatrix(DenseMatrix64F selectionMatrix) { inverseJacobianSolver.setSelectionMatrix(selectionMatrix); }
public boolean solveUsingJacobianPseudoInverseTwo(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseOne(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J^T J computeJacobianTransposedTimesJacobian(jacobianTransposedTimesJacobianMatrix, subspaceJacobianMatrix); // J^T*deltaX jacobianTimesSpatialVelocity.reshape(numberOfDoF, 1); CommonOps.mult(jacobianMatrixTransposed, subspaceSpatialVelocity, jacobianTimesSpatialVelocity); boolean success = linearAlgebraSolver.setA(jacobianTransposedTimesJacobianMatrix); // Solve J^T*J delta q = J^T * deltaX if (success) linearAlgebraSolver.solve(jacobianTimesSpatialVelocity, jointspaceVelocity); return success; }
public boolean solveUsingJacobianInverse(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); boolean success = linearAlgebraSolver.setA(subspaceJacobianMatrix); if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, jointspaceVelocity); return success; }
computePrivilegedVelocitiesForStayingAwayFromJointLimits(privilegedJointVelocities, jointAngleRegularizationWeight.getDoubleValue()); inverseJacobianSolver.solveUsingNullspaceMethod(spatialDesiredVelocity, jacobian.getJacobianMatrix(), privilegedJointVelocities); desiredJointVelocities.set(inverseJacobianSolver.getJointspaceVelocity()); subspaceSpatialError.reshape(inverseJacobianSolver.getNumberOfConstraints(), 1); subspaceSpatialError.set(inverseJacobianSolver.getSubspaceSpatialVelocity()); CommonOps.scale(controlDT, subspaceSpatialError);
private void computeJointAngleCorrection(DenseMatrix64F spatialError) { // inverseJacobianCalculator.solveUsingJacobianInverse(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseOne(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseTwo(spatialError); inverseJacobianCalculator.solveUsingDampedLeastSquares(spatialError, jacobian.getJacobianMatrix(), lambdaLeastSquares); jointAnglesCorrection.set(inverseJacobianCalculator.getJointspaceVelocity()); double correctionScale = RandomTools.generateRandomDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, jointAnglesCorrection); for (int i = 0; i < jointAnglesCorrection.getNumRows(); i++) { jointAnglesCorrection.set(i, 0, Math.min(maxStepSize, Math.max(jointAnglesCorrection.get(i, 0), -maxStepSize))); } }
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); }
private void updateBest() { errorScalar = NormOps.normP2(inverseJacobianCalculator.getSubspaceSpatialVelocity()); if (errorScalar < minimumErrorScalar) { getJointAngles(jointAnglesMinimumError); minimumErrorScalar = errorScalar; } }
/** * Solves the desired transform for the joint angles using the equation: δq = J<sup>T</sup>(JJ<sup>T</sup>)<sup>-1</sup> δX, with: * <li> δq is the correction in joint space </li> * <li> J is the Jacobian </li> * <li> δX is the spatial (orientation and position) error in taskspace </li> */ @Override public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; minimumErrorScalar = Double.POSITIVE_INFINITY; numberOfConstraints = inverseJacobianCalculator.getNumberOfConstraints(); boolean hasReachedMaxIterations = false; boolean hasReachedTolerance = false; do { computeError(desiredTransform); computeJointAngleCorrection(spatialError); updateBest(); applyJointAngleCorrection(); iterationNumber++; hasReachedMaxIterations = iterationNumber >= maxIterations; hasReachedTolerance = errorScalar <= tolerance; } while (!hasReachedTolerance && !hasReachedMaxIterations); updateBest(); setJointAngles(jointAnglesMinimumError); return hasReachedTolerance; }
public double computeDeterminant(DenseMatrix64F jacobianMatrix) { computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, jacobianMatrix); return CommonOps.det(jacobianTimesJacobianTransposedMatrix); }
private void computeJointAngleCorrection(DenseMatrix64F spatialError) { // inverseJacobianCalculator.solveUsingJacobianInverse(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseOne(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseTwo(spatialError); inverseJacobianCalculator.solveUsingDampedLeastSquares(spatialError, jacobian.getJacobianMatrix(), lambdaLeastSquares); jointAnglesCorrection.set(inverseJacobianCalculator.getJointspaceVelocity()); double correctionScale = RandomNumbers.nextDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, jointAnglesCorrection); for (int i = 0; i < jointAnglesCorrection.getNumRows(); i++) { jointAnglesCorrection.set(i, 0, Math.min(maxStepSize, Math.max(jointAnglesCorrection.get(i, 0), -maxStepSize))); } }
public boolean solveUsingJacobianInverse(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); boolean success = linearAlgebraSolver.setA(subspaceJacobianMatrix); if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, jointspaceVelocity); return success; }
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); }
private void updateBest() { errorScalar = NormOps.normP2(inverseJacobianCalculator.getSubspaceSpatialVelocity()); if (errorScalar < minimumErrorScalar) { getJointAngles(jointAnglesMinimumError); minimumErrorScalar = errorScalar; } }
/** * Solves the desired transform for the joint angles using the equation: δq = J<sup>T</sup>(JJ<sup>T</sup>)<sup>-1</sup> δX, with: * <li> δq is the correction in joint space </li> * <li> J is the Jacobian </li> * <li> δX is the spatial (orientation and position) error in taskspace </li> */ @Override public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; minimumErrorScalar = Double.POSITIVE_INFINITY; numberOfConstraints = inverseJacobianCalculator.getNumberOfConstraints(); boolean hasReachedMaxIterations = false; boolean hasReachedTolerance = false; do { computeError(desiredTransform); computeJointAngleCorrection(spatialError); updateBest(); applyJointAngleCorrection(); iterationNumber++; hasReachedMaxIterations = iterationNumber >= maxIterations; hasReachedTolerance = errorScalar <= tolerance; } while (!hasReachedTolerance && !hasReachedMaxIterations); updateBest(); setJointAngles(jointAnglesMinimumError); return hasReachedTolerance; }
public double computeDeterminant(DenseMatrix64F jacobianMatrix) { computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, jacobianMatrix); return CommonOps.det(jacobianTimesJacobianTransposedMatrix); }
public boolean solveUsingJacobianPseudoInverseTwo(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseOne(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J^T J computeJacobianTransposedTimesJacobian(jacobianTransposedTimesJacobianMatrix, subspaceJacobianMatrix); // J^T*deltaX jacobianTimesSpatialVelocity.reshape(numberOfDoF, 1); CommonOps.mult(jacobianMatrixTransposed, subspaceSpatialVelocity, jacobianTimesSpatialVelocity); boolean success = linearAlgebraSolver.setA(jacobianTransposedTimesJacobianMatrix); // Solve J^T*J delta q = J^T * deltaX if (success) linearAlgebraSolver.solve(jacobianTimesSpatialVelocity, jointspaceVelocity); return success; }
public void setSelectionMatrix(DenseMatrix64F selectionMatrix) { if (selectionMatrix.getNumCols() != SpatialMotionVector.SIZE) throw new RuntimeException("The selection matrix must have 6 columns, the argument has: " + selectionMatrix.getNumCols()); inverseJacobianCalculator.setSelectionMatrix(selectionMatrix); }
solver = new YoSolvePseudoInverseSVDWithDampedLeastSquaresNearSingularities(namePrefix, maxNumberOfConstraints, maxNumberOfConstraints, registry); inverseJacobianSolver = new InverseJacobianSolver(maxNumberOfConstraints, numberOfDoF, solver);