private RandomRestartInverseKinematicsCalculator createCalculator(GeometricJacobian jacobian, int maxIterations) { double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; int maxRestarts = 20; double restartTolerance = 0.001; NumericalInverseKinematicsCalculator numericalInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); numericalInverseKinematicsCalculator.setLimitJointAngles(false); RandomRestartInverseKinematicsCalculator randomRestartInverseKinematicsCalculator = new RandomRestartInverseKinematicsCalculator(maxRestarts, restartTolerance, jacobian, numericalInverseKinematicsCalculator); return randomRestartInverseKinematicsCalculator; }
calculator.solve(desiredTransform); long tf = System.nanoTime(); long solutionTime = tf - t0; timeStatistics.addValue(Conversions.nanosecondsToSeconds(solutionTime)); iterationStatistics.addValue(calculator.getNumberOfIterations()); System.out.println("Initial Jacobian determinant: " + det); System.out.println("Current Jacobian determinant: " + jacobian.det()); System.out.println("Number of iterations: " + calculator.getNumberOfIterations()); System.out.println("Error: " + calculator.getErrorScalar()); System.out.print("Joint angles: ");
calculator.solve(desiredTransform); long tf = System.nanoTime(); long solutionTime = tf - t0; iterationStatistics.addValue(calculator.getNumberOfIterations());
double restartTolerance = 0.006; inverseKinematicsCalculator = new RandomRestartInverseKinematicsCalculator(maxRestarts, restartTolerance, leftHandJacobian, numericalInverseKinematicsCalculator);