Search for cheapest IK solution

[nalt]

• searchPositionIK() returned the first collision-free solution within joint limits it finds. (The free joint is moved as little as possible.) This approach regularly leads to large and ugly motions in joint space even for small displacements of the EE, see video [1]. Instead, the solution with the smallest costs should be returned. Here, the largest motion of any joint is taken as the cost term. Trajectories become much smoother, see video [2].
• Due to the speed of IK fast, it is ok to search the entire search space. However, if callbacks for collision checking are used, a coarse-to-fine search scheme might be needed.
• getCount() was called with a positive third argument. Thus, no search in negative direction was performed.

[1] https://www.youtube.com/watch?v=-VX709r0N2Y
[2] https://www.youtube.com/watch?v=BFpG8TY6aqk

[davetcoleman]

ROS/MoveIt/this code's style is to put opening brackets on new line

[davetcoleman]

Great improvement in the videos!

+1 I reviewed the code but didn't follow the changes' functionality 100%

[gavanderhoorn]

I like this too, but I'm just wondering whether we should make this configurable? I see some magic nrs in the added code, as well as a boolean. Those seem like excellent configurable items to me (with proper defaults).

Also, I'm a bit cautious: as this change would make newly generated plugins have different behaviour, should we make the defaults be the old behaviour?

[nalt]

Yes, bool search_best is there to make it configurable. It is the only configurable parameter right now. But should this be done during generation or runtime?
Concerning defaults: I had quite bad results (i.e. large motions in joint space) with the old code on the Kuka LWR and also on the ClamArm with MX servos. This may be less obvious if joint limits and collision constraints are stricter anyway. But, I do not think the old optimization - which minimizes the motion of the free joint only - is a very good default. Other IK methods (e.g. KDL) also give solutions which are close to the initial state (if they find anything at all...).
We could encourage the user to set some default by printing an error message.

[gavanderhoorn]

Yes, bool search_best is there to make it configurable. It is the only configurable parameter right now. But should this be done during generation or runtime?

Should the number of attempts not be considered as well?

Concerning defaults: I had quite bad results (i.e. large motions in joint space) with the old code on the Kuka LWR and also on the ClamArm with MX servos. This may be less obvious if joint limits and collision constraints are stricter anyway. But, I do not think the old optimization - which minimizes the motion of the free joint only - is a very good default. Other IK methods (e.g. KDL) also give solutions which are close to the initial state (if they find anything at all...).

I agree that the current behaviour (or old, after acceptance of this PR) is less than optimal. I may be a bit conservative, but I generally try to make behavioural changes like these -- which might not be apparent to end-users just upgrading their packages -- optional, or at least configurable.

The setting could then default to the new value after some time, giving people a chance to upgrade any code that depends on the current behaviour.

[shaun-edwards]

+1 to this improvement. This is a common failure we've seen in MoveIt. We've always assumed it came from the planners (maybe part of it still does).

+1 to preserving the old behavior if possible. I know it's extra work and seems counter-intuitive, but it's important to consider the potential effects to all the other users of MoveIt. Because this only happens at generation time, this is less likely but still possible.

My suggestion is to make the default behavior, the old behavior, and the new behavior enabled by parameter (looks like this is possible). In addition, I would log a strong warning that tells the user that there is a better IK solution and how to generate it. At the same time we should add an issue to make the new behavior the default in the next version.

[davetcoleman]

At the same time we should add an issue to make the new behavior the default in the next version.

I think we could make it default in indigo and it wouldn't affect anyone. The fact that this only changes behavior when a user re-creates their plugin is another safeguard we have - this won't change anyone's existing plugins. This looks like a really good change that we should make available!

[nalt]

The optimization method could be set using the options argument of searchPositionIK(). This would require to change the kinematics::KinematicsQueryOptions type. Right now, it has only 2 options.

Possible optimization goals are:

• Minimize the inf-norm of the joint movement (this pull request)
• Minimize the L2-norm of the joint movement
• Weigh the norm with the maximum joint speed for the above minimizations. This would basically minimize time, if joint speeds are different for different joints.
• Find a solution which is furthest from the joint limits. This would be helpful to find a good working point of the arm.

Do you think this is of general interest?

[gavanderhoorn]

@nalt wrote:

[..]
Do you think this is of general interest?

Yes, I think this is certainly of general interest, although most users will not touch any defaults we set. I've always wondered why the (IKFast) kinematics plugin was not configurable and just always returned the first solution. In comparison to other frameworks / libraries this seemed rather minimal (but functional!).

We have two options. Either we:

1. merge this PR as-is, and any further improvements (as the currently proposed changes are certainly an improvement) are done in new PRs
2. the additional goals you list are added to this one.

Personally, I think we should go with 1. This is a nice start, has been reviewed and it works. Further refinement / extension should be done in new PRs.

[nalt]

Search mode can now be specified during code generation (optional argument). Default is to minimize the maximal joint motion, but a warning is shown. The flags for search modes can easily be extended and eventually be included in the KinematicsQueryOptions type.

[davetcoleman]

I agree further features should be done in a new PR.

although most users will not touch any defaults we set

This is very true.

I vote we merge this, rename master to hydro-devel and make a new indigo-devel branch that has the "solution with the smallest costs should be returned" the default option. Note that you are welcome to use the Indigo branch of moveit_ikfast with Hydro (built from source) but we don't want to change a stable ROS release at this point.

Then open a new PR for future changes.

@gavanderhoorn feel free to merge

[gavanderhoorn]

Ok, let's do as @davetcoleman proposes. I'll make the necessary changes / additions.

[gavanderhoorn]

@davetcoleman: renaming master would also require updates to the rosdistro entries for moveit_ikfast, the release repository, as well as the wiki. I can do the rosdistro and wiki bits, could you fix the releases?

[davetcoleman]

Yes, I can fix the releases but I might have to, at the same time, release a new debian.

[gavanderhoorn]

Yes, I assumed 'fixing releases' would include having to release a new version.

[davetcoleman]

Released

[gavanderhoorn]

Tnx.