Add approximate solution joint jump threshold parameter

[sea-bass]

We have seen some joint jumps when using pick_ik with MoveIt Servo, where approximate solutions are enabled.

This seems to me like a workaround to a deeper bug that we haven't yet found, so keeping in draft for now.

[tylerjw]

This seems good if it is useful in your testing. Let me know when you'd like me to cut a release.

[tylerjw]

I see the ci failures and will fix those for you separately.

[sea-bass]

I see the ci failures and will fix those for you separately.

I think I just fixed it, was just testing this branch on an older version of pick_ik.

Not sure if I want to merge this yet, so let's hang on until I take this out of draft? When I'm back from vacation I may want to spend a little more time chasing down the root cause instead of slapping yet another parameter on this, if possible.

Specifically, I'm not entirely sure why the position/orientation thresholds passed when the arm was clearly teleporting a large distance, and why the joint angle check is even needed.

[sea-bass]

I took a closer look and everything seems in order... I even checked the thread-safeness of the forward kinematics function thinking maybe something was off there?

This might just be a necessary parameter because approx. solutions with the global solver can really make the arm teleport to a redundant solution.

[sea-bass]

I finally found what's going on. It's actually nothing to do with pick_ik, so let's for sure merge this @tylerjw.

When MoveIt servo is near a singularity, the Jacobian based twist computation puts the target pose to really weird places. When using a local solver like the built-in one in Servo, KDL, or even pick_ik without the memetic solver, then the IK simply fails and returns the original solution. But pick_ik global solver returns an approximate solution that is closer to this garbage pose.

So... not to brag or anything, but pick_ik is just too good for MoveIt Servo 😎

anyways, adding in this extra "distance from initial joint config" parameter helps protect against these big jumps when the end effector is where it was commanded, but that just happens to be really far from the robot's current state.