randomFloatingChain.getLeafBody().getBodyFixedFrame()); DdoglegInverseKinematicsCalculator calculator = new DdoglegInverseKinematicsCalculator(jacobian, 1.0, 2000, true, 1e-12,0.01,0.1, 0.0); 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: ");
randomFloatingChain.getLeafBody().getBodyFixedFrame()); DdoglegInverseKinematicsCalculator calculator = new DdoglegInverseKinematicsCalculator(jacobian, 1.0, 2000, true, 1e-12,0.01,0.1, 0.1); calculator.solve(desiredTransform); long tf = System.nanoTime(); long solutionTime = tf - t0; iterationStatistics.addValue(calculator.getNumberOfIterations());
extractTandR(desiredTransform, desiredT, desiredR); updateState(bestParam); extractTandR(actualTransform, actualT, actualR);
double acceptTolAngle = 0.02; double parameterChangePenalty = 1.0e-4; //0.1; inverseKinematicsCalculator = new DdoglegInverseKinematicsCalculator(leftHandJacobian, orientationDiscount, maxIterations, solveOrientation, convergeTolerance, acceptTolLoc, acceptTolAngle, parameterChangePenalty);
extractTandR(desiredTransform, desiredT, desiredR); updateState(bestParam); extractTandR(actualTransform, actualT, actualR);