public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; bestErrorScalar = Double.POSITIVE_INFINITY; do { computeError(desiredTransform); updateBest(); computeCorrection(); applyCorrection(); iterationNumber++; } while ((errorScalar > tolerance) && (iterationNumber < maxIterations)); updateBest(); setJointAngles(best); if (iterationNumber < maxIterations) { useSeed = false; return true; } if (iterationNumber >= maxIterations && counter++ < 100) { introduceRandomArmePose(desiredTransform); } counter = 0; return false; }
private void updateBest() { getJointAngles(current); if (errorScalar < bestErrorScalar) { best.set(current); bestErrorScalar = errorScalar; } }
public void introduceRandomArmePose(RigidBodyTransform desiredTransform) { for (int i = 0; i < oneDoFJoints.length; i++) { oneDoFJoints[i].setQ(random.nextDouble()); } solve(desiredTransform); }
private void computeError(RigidBodyTransform desiredTransform) { /* * B is base E is end effector D is desired end effector * * H^D_E = H^D_B * H^B_E = (H^B_D)^-1 * H^B_E * * convert rotation matrix part to rotation vector */ jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, error); errorTranslationVector.get(3, error); errorScalar = NormOps.normP2(error); assert (exponentialCoordinatesOK()); }
public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; bestErrorScalar = Double.POSITIVE_INFINITY; do { computeError(desiredTransform); updateBest(); computeCorrection(); applyCorrection(); iterationNumber++; } while ((errorScalar > tolerance) && (iterationNumber < maxIterations)); updateBest(); setJointAngles(best); if (iterationNumber < maxIterations) { useSeed = false; return true; } if (iterationNumber >= maxIterations && counter++ < 100) { introduceRandomArmePose(desiredTransform); } counter = 0; return false; }
public void introduceRandomArmePose(RigidBodyTransform desiredTransform) { for (int i = 0; i < oneDoFJoints.length; i++) { oneDoFJoints[i].setQ(random.nextDouble()); } solve(desiredTransform); }
private void updateBest() { getJointAngles(current); if (errorScalar < bestErrorScalar) { best.set(current); bestErrorScalar = errorScalar; } }