Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

KDL IK solver improvements #1321

Merged
merged 7 commits into from Feb 4, 2019

Conversation

Projects
None yet
3 participants
@rhaschke
Copy link
Contributor

rhaschke commented Jan 23, 2019

This PR addresses #1255, a regression of KDL-IK performance in Melodic. I didn't find any particular reason for the regression, but I noticed the following general issues:

  • The IK solver was often stuck when running into joint limits (wasting most cycles trying to move outside the limits and being reset inside again).
  • Singularities were not handled gracefully.

These issues are addressed as follows:

  • Decrease step size when error-to-target has increased since last iteration (which is usually also an indicator for a singularity).
  • When getting stuck passing a singularity, try to (locally!) wiggle all joints or bail out if target cannot be reached, thus not wasting more cycles to reach an unreachable goal.
  • When joint values are clipped back to allowed range, reduce their motion contribution for the next cycle by using a weighted Jacobian. This allows the solver to find an alternative solution in the redundant space (if there is one). If there is no progress due to joint clipping, bail out.
  • The outer loop, might reach the goal from another randomly chosen seed.

The solver is now able to pass a singularity without re-seeding (and thus having large joint-space jumps):
singular

rhaschke added some commits Jan 4, 2019

locally wiggle joints when stuck in singularity
Instead of bailing out and re-seeding globally, first try to wiggle joints locally only
to overcome local minimum of gradient search.
clipToJointLimits()
- clip delta_q to joint limits before checking for convergence
- reduce joint_weighting for clipped joints to avoid moving further beyond bounds

@rhaschke rhaschke requested a review from mlautman Jan 30, 2019

@henningkayser
Copy link
Contributor

henningkayser left a comment

I tested this with a UR5 setup and changes definitely make the interactive marker in RViz a lot smoother.
I played around with some parameters and would only suggest reducing step size even stronger in case the error increases. (see below)

{
// if the error increased, we are close to a singularity -> reduce step size
double old_step_size = step_size;
step_size *= std::min(0.5, last_delta_twist_norm / delta_twist_norm); // reduce scale;

This comment has been minimized.

@henningkayser

henningkayser Jan 31, 2019

Contributor

I figured using a lower min value (i.e. 0.2) reduces the time being stuck at singularities.
This can be related to the setup, but is there another reason why this shouldn't be lower?

This comment has been minimized.

@rhaschke

rhaschke Feb 1, 2019

Author Contributor

This is a rather uncritical value. Choosing it too high, simply requires several iterations to actually decrease the error. Choosing it too small, might unnecessarily slow down convergence.
Everything staying in the same order of magnitude is fine. I will try myself on our robot zoo and then merge.

@jonbinney

This comment has been minimized.

Copy link
Contributor

jonbinney commented Feb 1, 2019

Do we have any tests that capture the effect of this change quantitatively? (e.g. before we were able to solve for X% of tool poses, but now we can solve for Y%)?

@rhaschke

This comment has been minimized.

Copy link
Contributor Author

rhaschke commented Feb 1, 2019

@jonbinney It's hard to devise meaningful tests for this. Your kinematic_tests approach, using random Cartesian goals is not very expressive, because it's prone to random bias. What we would need is a deterministic sampling of the whole workspace.

@rhaschke rhaschke merged commit f937cbb into ros-planning:melodic-devel Feb 4, 2019

@rhaschke rhaschke deleted the ubi-agni:kdl-improvements branch Feb 4, 2019

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.