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

ikfast improvements #1320

Merged
merged 12 commits into from Feb 1, 2019

Conversation

Projects
None yet
5 participants
@rhaschke
Copy link
Collaborator

commented Jan 23, 2019

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

  • fixing the issues already raised in #1014
  • including adaptions for tf2 transition done in Melodic
  • removing unused functions getClosestSolution() + harmonize()
  • adapting to new KinematicsBase API, thus addressing #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 ros-planning/moveit_tutorials#186 and ros-planning/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 rhaschke requested a review from mlautman Jan 23, 2019

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from 994e656 to d402bed Jan 23, 2019

@rhaschke

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 23, 2019

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.

@rhaschke rhaschke referenced this pull request Jan 23, 2019

Merged

BEFORE_DOCKER_SCRIPT #42

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch 3 times, most recently from 2cb5c5b to f942d41 Jan 23, 2019

@davetcoleman
Copy link
Member

left a comment

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

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 24, 2019

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(-)

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from 0eb988a to 2792e6b Jan 24, 2019

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Jan 24, 2019

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

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from 2792e6b to a114e83 Jan 24, 2019

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

commented Jan 24, 2019

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

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 24, 2019

@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 comment has been minimized.

Copy link
Contributor

commented Jan 24, 2019

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

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 24, 2019

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

This comment has been minimized.

Copy link
Contributor

commented Jan 24, 2019

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

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 24, 2019

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.

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch 2 times, most recently from 516c717 to a114e83 Jan 24, 2019

@mlautman
Copy link
Member

left a comment

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

Show resolved Hide resolved .travis.yml Outdated
Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh
Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh Outdated
}

function parse_options {
# set defaults (if not set in environment yet)

This comment has been minimized.

Copy link
@mlautman

mlautman Jan 24, 2019

Member

replace tabs with spaces

This comment has been minimized.

Copy link
@rhaschke

rhaschke Jan 24, 2019

Author Collaborator

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

This comment has been minimized.

Copy link
@davetcoleman

davetcoleman Jan 25, 2019

Member

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.

Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh Outdated
Show resolved Hide resolved moveit_kinematics/test/fanuc-kdl.test
Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh
Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh Outdated
Show resolved Hide resolved ...ics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh
Show resolved Hide resolved ...nematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py Outdated

mlautman and others added some commits Aug 1, 2018

cherry-pick ikfast improvements (#1014)
Generalize ikfast solver by allowing arbitrary base and tip links (different from the original ikfast chain)
as long as those links are rigidly connected to the chain's base resp. tip.
modularize create_ikfast_moveit_plugin.py
Split monolithic function update_package() into several smaller functions
become ROS agnostic
So far, only roslib.packages.get_pkg_dir() was used from ROS to find package paths.
Providing a stub for it, we can run the script also without a ROS context.
In this case, however, we cannot modify the MoveIt config package.
adapt to new KinematicsBase API
- initialize() from RobotModel
- rename active_ -> initialized_
fix compiler warnings
... about unused variables, mismatching sign comparison, etc.

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from b95fa79 to 5e6c99b Jan 29, 2019

@rhaschke

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 29, 2019

+1 after resolving the docker run comment and getting travis to pass.

@mlautman I slightly extended the docker-based creation script for Travis. It can take an option --from-cpp and then take the fast lane... I will keep the docker-based generation as a separate test case.
Just to check this PR against latest moveit_ci, I added b95fa79. This should be removed before merging.
If Travis passes, please finally validate and approve. Then I will merge this one after ros-planning/moveit_ci#45 was merged.

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from 5e6c99b to c1949b5 Jan 30, 2019

@rhaschke

This comment has been minimized.

Copy link
Collaborator Author

commented Jan 30, 2019

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.

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch 3 times, most recently from 9801a2b to 2e44219 Jan 30, 2019

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented Jan 30, 2019

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.

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch 6 times, most recently from 4c33aef to 98d1b13 Jan 30, 2019

@davetcoleman

This comment has been minimized.

Copy link
Member

commented Jan 30, 2019

gitlfs would solve the large file issue!

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch 7 times, most recently from 11ff12e to 6c26ed9 Jan 31, 2019

@rhaschke rhaschke force-pushed the ubi-agni:melodic-ikfast branch from 6c26ed9 to 3635a41 Jan 31, 2019

@rhaschke rhaschke merged commit 3635a41 into ros-planning:melodic-devel Feb 1, 2019

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

rhaschke added a commit that referenced this pull request Feb 1, 2019

Merge pull request #1320 from ubi-agni/ikfast-improvements
Adapt ikfast plugin to new KinematicsBase API.
Unit testing for ikfast solvers of fanuc + panda.

@rhaschke rhaschke deleted the ubi-agni:melodic-ikfast branch Feb 1, 2019

@rhaschke rhaschke referenced this pull request Feb 1, 2019

Closed

IKFast improvements #1014

3 of 5 tasks complete
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.