Added motoman_mh5 IKfast plugin

[nsnitish]

@shaun-edwards! Please have a look

[shaun-edwards]

@nsnitish, The failure occurs in the indigo build. The problem is an ambiguous call to isnan. The easiest solution is to add the std:: namespace to the call of isnan in `*_ikfast_solver, as was done here.

This would require changing an auto-generated ikfast file (not desirable).

@gavanderhoorn solved this problem, this way. However, when I use this, I get the following error:

motoman_mh5_manipulator_ikfast_solver.cpp:1631:77: error: ‘>>’ should be ‘> >’ within a nested template argument list
                                                                       IkReal>>

<snip>

motoman_mh5_manipulator_ikfast_moveit_plugin.cpp:1222:36: error: ‘>>’ should be ‘> >’ within a nested template argument list
   std::vector<IkSolutionList<IkReal>> solution_set;

This doesn't occur in the fanuc package, because it's *_ikfast_solver.cpp and *_moveit_plugin.cpp do not have examples of >>. I'm not sure why this is.

Unless @gavanderhoorn provides some insight, I suggest adding std:: to each isnan call.

[gavanderhoorn]

@shaun-edwards wrote:

This doesn't occur in the fanuc package, because it's *_ikfast_solver.cpp and *_moveit_plugin.cpp do not have examples of >>. I'm not sure why this is.

The IK plugins in fanuc are rather old (they're overdue for some updates: ros-industrial/fanuc#123), so the >> and isnan(..) stuff was probably added to the IKFast template after they were generated.

[nsnitish]

@shaun-edwards! Can you have a look?

[nsnitish]

@shaun-edwards! Are there any other changes required?

[gavanderhoorn]

Just curious: the generated motoman_mh5_manipulator_ikfast_solver.cpp appears to be 4.3MB (!) in size. That seems a tad excessive.

@nsnitish: can you describe how you generated this solver?

[gavanderhoorn]

Two other comments:

• update_ikfast_plugin.sh contains references to paths local to @nsnitish' machine (/home/nitishsharma/..)
• the introduced pkg does not follow the layout and formatting of other MoveIt IKFast plugins we have (in the ABB and Fanuc repositories fi). Would we want to harmonize across all ros-i repositories and update this one?

[nsnitish]

@gavanderhoorn! I followed this tutorial.

[gavanderhoorn]

And you installed OpenRAVE from source? Can you tell me which commit your openrave clone is at? What version of sympy did you use (dpkg -l | grep sympy)?

Which links did you select for baselink and eelink?

I've just run the same command, but using the personalrobotics/ros-openrave Docker image, and the generated IK solver is 103KB. That is quite a significant difference.

Running the iktests (with a different generator command) I get the following output:

testik, success rate: 1.000000, wrong solutions: 0.000000, no solutions: 0.000000, missing solution: 0.455000

That is not really good: basically the solver can only find a solution for about 50% of the queries (but if it finds one, they are all ok).

Have you ran the OpenRAVE iktests?

[nsnitish]

@gavanderhoorn! Thanks for pointing this out. It's kinda weird but the problem is in the clang formatting that I used. Auto-generated file is 571 KB.

I haven't ran the OpenRAVE iktests but I have tested the plugin with moveit_kinematics_tests.

[shaun-edwards]

It's odd that ikfast wouldn't be better at solving the mh5 kinematics. It is a standard industrial manipulator with a spherical wrist. If ikfast has trouble with this robot, then I would expect it to have trouble with a lot of other robots we have.

[gavanderhoorn]

@nsnitish wrote:

@gavanderhoorn! Thanks for pointing this out. It's kinda weird but the problem is in the clang formatting that I used. Auto-generated file is 571 KB.

So that would mean that the formatting introduced about 3.8MB of additional characters?

I haven't ran the OpenRAVE iktests but I have tested the plugin with moveit_kinematics_tests.

And that all succeeded?

It seems strange that OpenRAVE's own tests would indicate this low a success rate, and then 'our' test does succeed. It's not impossible though, as I've seen other people claim that iktests failed horribly for them, but the solver worked 'alright'.

But I think this deserves some more investigation.

@nsnitish: could you also answer my other questions please?

[gavanderhoorn]

@shaun-edwards:

It's odd that ikfast wouldn't be better at solving the mh5 kinematics. It is a standard industrial manipulator with a spherical wrist. If ikfast has trouble with this robot, then I would expect it to have trouble with a lot of other robots we have.

I agree that it probably should be better, but I'm just reporting what I'm seeing.

If the generated solver is really not that good, we should at least mention that somewhere.

Related: ros-industrial/fanuc#99.

[nsnitish]

@gavanderhoorn!

1. I am using the latest stable commit of openRAVE, sympy version 1.1.

2. baselink is 0 and eelink is 6.

3. Results from moveit_kinematics_tests for motoman_mh5

[gavanderhoorn]

@nsnitish wrote:

1. I am using the latest stable commit of openRAVE,

wow. That is quite old: latest_stable...master.

sympy version 1.1.

I'm surprised this works. But maybe there is such a difference between latest-stable and master that my previous experience with sympy doesn't apply here.

baselink is 0 and eelink is 6.

ok.

Results from moveit_kinematics_tests for motoman_mh5

Ok. This doesn't tell me much but it looks like most tests are succeeding.

I still believe this deserves more investigation, as it's at least weird that OpenRAVE's own test claims that 50% of IK requests fail, while 'our' tests indicate that (almost) everything is fine.

[gavanderhoorn]

@nsnitish wrote:

@gavanderhoorn! Thanks for pointing this out. It's kinda weird but the problem is in the clang formatting that I used. Auto-generated file is 571 KB.

this is unfortunate actually: we don't require clang-formatting in this repository, and it seems strange to do that to auto-generated code in the first place. But it's been merged, so lets not concern ourselves with that any more.