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; }
foundSolution = inverseKinematicsCalculator.solve(desiredTransform); double errorScalar = inverseKinematicsCalculator.getErrorScalar(); foundSolution = foundSolution || (errorScalar < restartTolerance); inverseKinematicsCalculator.getBest(best); inverseKinematicsCalculator.setJointAngles(best); return foundSolution;
/** * 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; }
private boolean isPositionReachable(int numberOfTrials, int xIndex, int yIndex, int zIndex) { boolean success = false; int counter = 0; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); voxel3dGrid.getVoxel(voxelLocation, xIndex, yIndex, zIndex); modifiableVoxelLocation.setIncludingFrame(voxelLocation); modifiableVoxelLocation.changeFrame(jacobian.getBaseFrame()); desiredEndEffectorPose.setTranslation(modifiableVoxelLocation.getX(), modifiableVoxelLocation.getY(), modifiableVoxelLocation.getZ()); while (counter < numberOfTrials) { success = linearInverseKinematicsCalculator.solve(desiredEndEffectorPose); if (success) break; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); counter++; } return success; } }
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); }
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; }
@Override public void setLimitJointAngles(boolean limitJointAngles) { inverseKinematicsCalculator.setLimitJointAngles(limitJointAngles); }
@Override public void attachInverseKinematicsStepListener(InverseKinematicsStepListener stepListener) { inverseKinematicsCalculator.attachInverseKinematicsStepListener(stepListener); }
boolean success = inverseKinematicsForLowerArms.get(robotSide).solve(desiredTransformForLowerArm);
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); }
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); inverseKinematicsForUpperArms.put(robotSide, inverseKinematicsForUpperArm); 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); inverseKinematicsForLowerArms.put(robotSide, inverseKinematicsForLowerArm);
@Override public void setLimitJointAngles(boolean limitJointAngles) { inverseKinematicsCalculator.setLimitJointAngles(limitJointAngles); }
@Override public void attachInverseKinematicsStepListener(InverseKinematicsStepListener stepListener) { inverseKinematicsCalculator.attachInverseKinematicsStepListener(stepListener); }
boolean success = inverseKinematicsForLowerArms.get(robotSide).solve(desiredTransformForLowerArm);
double maxRandomSearchScalar = 0.8; NumericalInverseKinematicsCalculator numericalInverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(leftHandJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForUpperArm = new NumericalInverseKinematicsCalculator(upperArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsForUpperArm.setSelectionMatrix(angularSelectionMatrix); inverseKinematicsForUpperArms.put(robotSide, inverseKinematicsForUpperArm); lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForLowerArm = new NumericalInverseKinematicsCalculator(lowerArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsForLowerArm.setSelectionMatrix(angularSelectionMatrix); inverseKinematicsForLowerArms.put(robotSide, inverseKinematicsForLowerArm);
jacobian = new GeometricJacobian(ikJoints, ikJoints[ikJoints.length - 1].getSuccessor().getBodyFixedFrame()); inverseKinematicsCalculator = new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterationsForIK, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); inverseKinematicsCalculator.setLimitJointAngles(true);
foundSolution = inverseKinematicsCalculator.solve(desiredTransform); double errorScalar = inverseKinematicsCalculator.getErrorScalar(); foundSolution = foundSolution || (errorScalar < restartTolerance); inverseKinematicsCalculator.getBest(best); inverseKinematicsCalculator.setJointAngles(best); return foundSolution;
/** * 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 void variableChanged(YoVariable<?> v) { if (legInverseKinematicsCalculators == null) return; reader.read(); sdfRobot.update(); if (sliderSpace.getEnumValue() == SliderSpace.CARTESIAN) { for (RobotSide robotSide : RobotSide.values()) { YoFramePose footIK = feetIKs.get(robotSide); FramePoint position = footIK.getPosition().getFramePointCopy(); FrameOrientation orientation = footIK.getOrientation().getFrameOrientationCopy(); FramePose framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); framePose.getPose(desiredTransform); legInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); YoFramePose handIK = handIKs.get(robotSide); position = handIK.getPosition().getFramePointCopy(); orientation = handIK.getOrientation().getFrameOrientationCopy(); framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); framePose.getPose(desiredTransform); armInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); } writer.updateRobotConfigurationBasedOnFullRobotModel(); } } }