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; }
public static NumericalInverseKinematicsCalculator createIKCalculator(JointBasics[] jointsToControl, int maxIterations) { JointBasics[] cloneOfControlledJoints = MultiBodySystemFactories.cloneKinematicChain(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBodyBasics cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
public static NumericalInverseKinematicsCalculator createIKCalculator(InverseDynamicsJoint[] jointsToControl, int maxIterations) { InverseDynamicsJoint[] cloneOfControlledJoints = ScrewTools.cloneJointPath(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBody cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
private NumericalInverseKinematicsCalculator createNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, int maxIterations, boolean doOrientation) { double tolerance = 1e-8; double maxStepSize = 0.2; double lambdaLeastSquares = 0.0009; double minRandomSearchScalar = 0.01; double maxRandomSearchScalar = 0.8; DenseMatrix64F selectionMatrix; if (doOrientation) selectionMatrix = CommonOps.identity(SpatialMotionVector.SIZE); else { selectionMatrix = new DenseMatrix64F(3, SpatialMotionVector.SIZE); selectionMatrix.set(0, 3, 1.0); selectionMatrix.set(1, 4, 1.0); selectionMatrix.set(2, 5, 1.0); } NumericalInverseKinematicsCalculator numericalInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); numericalInverseKinematicsCalculator.setSelectionMatrix(selectionMatrix); return numericalInverseKinematicsCalculator; }
double maxRandomSearchScalar = 0.8; NumericalInverseKinematicsCalculator numericalInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(leftHandJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForUpperArm = new NumericalInverseKinematicsCalculator(upperArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsForUpperArm.setSelectionMatrix(angularSelectionMatrix); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide), lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForLowerArm = new NumericalInverseKinematicsCalculator(lowerArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsForLowerArm.setSelectionMatrix(angularSelectionMatrix);
upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForUpperArm = new NumericalInverseKinematicsCalculator(upperArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForLowerArm = new NumericalInverseKinematicsCalculator(lowerArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar,
GeometricJacobian armJacobian = new GeometricJacobian(chest, hand, hand.getBodyFixedFrame()); NumericalInverseKinematicsCalculator legInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(legJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); NumericalInverseKinematicsCalculator armInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(armJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
GeometricJacobian armJacobian = new GeometricJacobian(chest, hand, hand.getBodyFixedFrame()); NumericalInverseKinematicsCalculator legInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(legJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); NumericalInverseKinematicsCalculator armInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(armJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
GeometricJacobian armJacobian = new GeometricJacobian(chest, hand, hand.getBodyFixedFrame()); NumericalInverseKinematicsCalculator legInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(legJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); NumericalInverseKinematicsCalculator armInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(armJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
jacobian = new GeometricJacobian(ikJoints, ikJoints[ikJoints.length - 1].getSuccessor().getBodyFixedFrame()); inverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterationsForIK, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsCalculator.setLimitJointAngles(true);