ikfast improvements

[rhaschke]

This is a rebase of @mlautman's #1014 to Melodic

• fixing the issues already raised in IKFast improvements #1014
• including adaptions for tf2 transition done in Melodic
• removing unused functions getClosestSolution() + harmonize()
• adapting to new KinematicsBase API, thus addressing Migrate IK Fast template to new KinematicsPlugin API #1262
• fixing compiler warnings in ikfast wrapper
• introducing robot-specific C++ namespaces to allow loading of multiple (different) ikfast plugins
• and, last but not least, providing a unit test mechanism for the ikfast wrapper

The unittest utilizes a docker container for OpenRave (integrated with ROS Indigo) as suggested in moveit/moveit_tutorials#186 and moveit/moveit_tutorials#187. There were several options available to realize unit tests, none of them being optimal:

• Configuring the ikfast plugins from catkin. Would be elegant, but catkin cannot handle a new package popping up during the running build process.
• Configuring them only for Travis. Tests will run only if the ikfast plugin packages are found.
  • Using a moveit_ci BEFORE_SCRIPT would require to run docker within docker, which is not recommended.
  • Running as a Travis before_script requires the URDFs from moveit_resources to be available,
    when the repo isn't yet cloned from moveit.rosinstall. Also, the Travis script runs w/o ROS context.

I decided for the last option, explicitly cloning moveit_resources beforehand and having the creator scripts made ROS agnostic.

When merging, please keep full history, i.e. create a merge commit.

[rhaschke]

Creating the ikfast plugins takes another 5-7min of time in Travis, which should be avoided to be done multiple times. I suggest to introduce a new variable BEFORE_DOCKER_SCRIPT which allows to specify a script to be run inside travis.sh before running docker.

[davetcoleman]

Really awesome contribution with the test coverage! And thanks @mlautman for making the bulk of these changes. A few small feedback items and then let's get this merged in finally.

[rhaschke]

Really awesome contribution with the test coverage! And thanks @mlautman for making the bulk of these changes.

I greatly appreciate Mike's initial contributions. But they are not the "bulk of these changes":

$ git diff --stat 6620f0f2247330273354ec419226a66fe5f92d9f^ 6620f0f2247330273354ec419226a66fe5f92d9f
 moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py        | 609 ++++++++++++++++++++++++++++++-------------------------
 moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt                      |  16 +-
 moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp | 297 +++++++++++++++++++--------
 3 files changed, 544 insertions(+), 378 deletions(-)


$ git diff --stat 6620f0f2247330273354ec419226a66fe5f92d9f melodic-ikfast 
 .travis.yml                                                                              |   2 +-
 moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py        | 403 ++++++++++++++++++++++++++++---------------------------
 moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.sh        | 212 +++++++++++++++++++++++++++++
 moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt                      |  10 +-
 moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp | 374 ++++++++++++++++-----------------------------------
 moveit_kinematics/test/CMakeLists.txt                                                    |  23 +++-
 moveit_kinematics/test/all.test                                                          |   5 -
 moveit_kinematics/test/create_ikfast_plugins.sh                                          |  14 ++
 moveit_kinematics/test/{fanuc.test => fanuc-ikfast.test}                                 |  10 +-
 moveit_kinematics/test/fanuc-kdl.test                                                    |  94 +++++++++++++
 moveit_kinematics/test/panda-ikfast.test                                                 |  65 +++++++++
 moveit_kinematics/test/{panda.test => panda-kdl.test}                                    |  18 ++-
 12 files changed, 751 insertions(+), 479 deletions(-)

[davetcoleman]

I'll defer to @mlautman for approval of this PR

[simonschmeisser]

Since you like disruptive changes @rhaschke I wonder if this ikfast mess should not be solved more generally. As I see it a ikfast plugin consists of two parts:

1. generic wrapper part that has the plugin API from KinematicsBase and depends upon a specific MoveIt! "release"
2. model specific generated cpp code with a fairly minimal API

at the moment 1. is generated as well and not updated automatically. There is no clear reason for this. However it creates maintenance issues at ROS-I as they a) would need separate branches for kinetic/indigo and melodic because MoveIt! changed API. Also b) changes like this need updates and PRs in at least 10 repos at ros-i. The solution [1] and [2] by @gavanderhoorn is to basically drop ikfast for ros-i and switch to opw_kinematics (which I totally support and will adapt for us).

So my idea would be to have

1. a generic ikfast plugin library which is either linked by the specific ikfast plugin (using inheritance maybe?) or

2. a generic "adapter" plugin that loads a minimal ikfast-specific plugin

(Note: For me/us I will still prefer opw_kinematics and I'm not going to implement any of this as long as I don't find a robot model that is supported by ikfast but not opw_kinematics ...)

1: ros-industrial/fanuc#241
2: ros-industrial/fanuc#245

[rhaschke]

@simonschmeisser Each ikfast plugin already creates an update script, which can be used to update the wrapper without touching the OpenRave code. I don't see the point in having an extra plugin/wrapper layer between the KinematicsBase plugin and OpenRave code. This will just move the API update issues to another layer.
By the way, this PR uses std/boost agnostic shared_ptrs to make the plugins work on both Kinetic and Melodic. You need to update your wrappers though, to benefit from this...

opw_kinematics seems to be interesting. Need to have a look.

[simonschmeisser]

This will just move the API update issues to another layer.

@rhaschke yes, but this layer would be here in the MoveIt! in a central place instead of repeated in 10+ downstream repos. And since the "openrave" API has been stable for quite some years the "issue" would actually vanish.

[rhaschke]

I see, you think of a generic KinematicsBase-IKFast plugin (provided once by MoveIt), which then loads the actual OpenRave solver as a plugin. Hence, we would be free to change the API of KinematicsBase.
I like the idea. This would be indeed a disruptive change ;-) You are welcome to file a PR for this.

[simonschmeisser]

It could either load it as plugin or just link it. See sketch below

In the generic part in moveit: (can be updated any time)

class GenericIkFastPlugin : public KinematicsBase
{
//usual functions using ik_fast_->
protected:
IkFastInterface ik_fast_;
};

class IkFastInterface
{
virtual getSolution(...) = 0;
};

and in the specific plugin (this will likely never need to be updated, just recompiled)

class Fanuc123IkFastPlugin : public GenericIkFastPlugin
{
   Fanuc123IkFastPlugin()
   {
      ik_fast_ = new Fanuc123IkFastInterface();
   }
}

class Fanuc123IkFastInterface : public IkFastInterface
{
   virtual getSolution(...) override
  {
    ikFast_getSolution(...)
  }
}

but no, sorry, I'm also not going to implement it, opw_kinematics ... ;)

[rhaschke]

Using inheritance would be even simpler, indeed. Shouldn't be that difficult to implement. But I don't have cylces anymore. Really need to focus on my own work now...
opw_kinematics is limited to parallel-base, spherical-wrist robots, though many industrial robots follow this design.

[mlautman]

These are great improvements! I am very excited for when this is merged :)

[mlautman]

replace tabs with spaces

[rhaschke]

Nope. I like spaces because they allow users to adjust their indentation.

[davetcoleman]

Spaces are the moveit and ros and google code style, which exist to eliminate these needless discussions. From ROS:

Indent each block by 2 spaces. Never insert literal tab characters.

[rhaschke]

It can take an option --from-cpp and then take the fast lane...

Just noticed that these OpenRave cpps are ~ 4 MB in total! I don't think we want to add them to the source repository just for fast unit testing.

$ ls -alh moveit_kinematics/test/*solver.cpp
-rw-r----- 1 rhaschke nistaff 2.5M Jan 30 01:15 moveit_kinematics/test/fanuc_manipulator_ikfast_solver.cpp
-rw-r----- 1 rhaschke nistaff 1.3M Jan 30 01:15 moveit_kinematics/test/panda_panda_arm_ikfast_solver.cpp

Maybe we can store them as a gist on https://gist.github.com/ or on some other external server and download them from there during Travis testing.

[gavanderhoorn]

Just noticed that these OpenRave cpps are ~ 4 MB in total! I don't think we want to add them to the source repository just for fast unit testing.

$ ls -alh moveit_kinematics/test/*solver.cpp
-rw-r----- 1 rhaschke nistaff 2.5M Jan 30 01:15 moveit_kinematics/test/fanuc_manipulator_ikfast_solver.cpp
-rw-r----- 1 rhaschke nistaff 1.3M Jan 30 01:15 moveit_kinematics/test/panda_panda_arm_ikfast_solver.cpp

That's interesting. The M-10iA IKFast solver over at ros-industrial/fanuc (this one) is only 104 KB.

[davetcoleman]

gitlfs would solve the large file issue!