IK: combine PRs #239, #305, and #316

[mxgrey]

There have been a number of issues and PRs raised regarding issues with the UR having multiple IK solutions. The existing IK solver only returns one single solution, and when determining that solution it will ignore many potential valid solutions.

PR #239 implements an interface to retrieve all valid solutions, but it does not account for all combinations of potentially valid solutions.

PR #305 accounts for all combinations of potentially valid solutions, including those that go past +/-pi. It does so in a way that is robust to changes in the model joint limits.

Since they're touching on similar parts of the code, they have some conflicts, so this PR merges them and resolves those conflicts.

PR #316 addresses the same issue as #305, but it only provides a single solution, whereas the interface introduced in #239 is looking for all solutions. #316's implementation is also tied to assumptions about how large the joint limits of the UR can be. Using the implementation of #305 instead of #316 allows us to tackle both issues (1) finding one best solution, and (2) finding all valid solutions with the same code. It's also future-proof in case the joint limits change (unless the UR ever gets a continuous joint with infinite range, but that would lead to infinite solutions). I would suggest passing on #316 unless it can be demonstrated with a benchmark that it offers considerable performance benefits.

[mxgrey]

Note that there are some concerns about the performance of this implementation. More information can be found here.

To address those concerns, I've created a branch that merges in the changes of #316. Reviewers are encouraged to use this performance test to compare the two options and decide which version is preferable.

If reviewers desire the approach of #316, I can merge that into this PR at any time.

[mxgrey]

As explained here, it's been determined that the approach of #316 performs much more favorably than #305 under some nominal conditions, so that approach has been merged into this PR as well.

[gavanderhoorn]

@mxgrey: thanks for the extensive descriptions and analyses. As I wasn't involved in any of this I'm going to need some help.

First: @hartmanndennis: would you be ok with this PR getting merged instead of your #316? With the last update by @mxgrey, it seems he's taken your critique into account and addressed the issue you raised. Is that correct?

@mxgrey: I'd like to keep the performance test you wrote around. Would it make sense to add the launch file and test prog to ur_kinematics?

Finally: @mxgrey: your last comment on #316 notes:

The only caveat (as I understand it) is this approach seems to make some assumptions about the outer limits of what the joint position limits can be. If the joint limits are ever extended past those assumptions, then someone will have to remember to tweak this implementation (although please do correct me if I'm misunderstanding that).

I've not looked at the code yet (I really should), but for now: are you referring to the fact that the code adds 2pi to solutions and assumed that is all it has to do (instead of potentially adding 2pi multiple times until it runs into a joint limit)? If so, I believe that would be acceptable for now.

[gavanderhoorn]

Additional comment: there appear to be quite a few places where magic nrs are introduced (5, 6). Those should probably be replaced with constants or values should be dynamically determined to avoid introducing assumptions about kinematic chains.

[mxgrey]

I'd like to keep the performance test you wrote around. Would it make sense to add the launch file and test prog to ur_kinematics?

Sure. They should probably be given more descriptive names, though.

are you referring to the fact that the code adds 2pi to solutions and assumed that is all it has to do (instead of potentially adding 2pi multiple times until it runs into a joint limit)?

That's exactly right.

[hartmanndennis]

I've not looked at the code yet (I really should), but for now: are you referring to the fact that the code adds 2pi to solutions and assumed that is all it has to do (instead of potentially adding 2pi multiple times until it runs into a joint limit)? If so, I believe that would be acceptable for now.

Yes it at most adds or subtracts 2pi currently. This could be changed without much effort to make it more general but I doubt the limits are ever going to be changed.

First: @hartmanndennis: would you be ok with this PR getting merged instead of your #316? With the last update by @mxgrey, it seems he's taken your critique into account and addressed the issue you raised. Is that correct?

I would prefer my PR getting merged. It only touches searchPosistionIK, which this PR doesn't have to make changes to.
Looking at the code of this PR right now my changes aren't even in here. It uses the enumerate all method in searchPositionIK as well.

[mxgrey]

Looking at the code of this PR right now my changes aren't even in here. It uses the enumerate all method in searchPositionIK as well.

@hartmanndennis Are you perhaps looking at an older version? The current version is using your method in searchPositionIK, starting at this line. The enumeratePeriodicSolutions function is only being used in getAllPositionIK.

This PR right now should effectively be #239, #305, and #316 all together, and with the merge conflicts resolved. If people would like to merge the PRs individually, that would certainly be doable, but whoever does the merging will have to re-resolve the conflicts.

[gavanderhoorn]

@mxgrey: I don't see any additional changes being pulled in by your last merge. Did something go wrong?

[mxgrey]

Ah, no, I was just double-checking that this branch was up-to-date.

[simonschmeisser]

Hi @mxgrey

I took a short look at your PR in hope of at some point approving either of the many open PRs so this can reach the quality of other analytical kinematic solvers (ik_fast, opw). Not sure if you are still around and willing to iterate on this but let's try :)

Besides the inline comments, this also needs a rebase due to bogus unused headers.

[mxgrey]

@simonschmeisser Thanks for catching those bugs, and sorry for my slow reply. This PR was from participating in a ROS-I Hackathon Day some years ago, so it's fallen very low on my priority list. But I looked over the suggested changes, and I think you're exactly right.

However, I've added one more change, which is to clear the solutions variable before having it get filled in. That helps to make sure that if the user is recycling the variable they use for the output argument, it doesn't keep accumulating new solutions on every call. I imagine the original intention was to have a solutions = q_ik_sols; line before the end of the function, but these changes accomplish the same thing, likely with less overhead.

[gavanderhoorn]

Ok, so retargetting doesn't immediately work. Unfortunately.

[dhled]

Hum, I will check that

[gavanderhoorn]

It's probably just github which is confused, as the changes are very much localised.

[akdhandy]

I am to know curious if this branch is still active or obsolete?

[fmauch]

I am to know curious if this branch is still active or obsolete?

No, this is not really active. Finishing ur_kinematics is our next goal for this repository. See also #599