Migrate to opw_kinematics for all supported robots

[gavanderhoorn]

Instead of IKFast use Jmeyer1292/opw_kinematics for all robot models and variants we support.

Current status:

• fanuc_cr35ia_support
• fanuc_cr7ia_support
• fanuc_lrmate200i_support
• fanuc_lrmate200ib_support
• fanuc_lrmate200ic_support (partial)
• fanuc_lrmate200id_support
• fanuc_m10ia_support
• fanuc_m16ib_support
• fanuc_m20ia_support
• fanuc_m20ib_support
• fanuc_m430ia_support
• fanuc_m6ib_support (partial)
• fanuc_m710ic_support
• fanuc_m900ia_support
• fanuc_m900ib_support
• fanuc_r1000ia_support

[simonschmeisser]

just came across one "small" issue ... Jmeyer1292/opw_kinematics#11

[gavanderhoorn]

Hm. I had not noticed that.

Let's see what @Jmeyer1292's response is.

[gavanderhoorn]

@Jmeyer1292 changed the license to Apache 2 so licensing would no longer seem to be an issue.

[simonschmeisser]

the MoveIt! plugin by @JeroenDM is Apache since yesterday as well and a first quick test worked quite nicely

[gavanderhoorn]

Did you observe the "low performance" that @JeroenDM was complaining about some time ago?

[simonschmeisser]

@gavanderhoorn it "feels sane" but I'm waiting for moveit/moveit#1272 to actually measure it. I have some other unrelated but huge performance issue that I need to profile first.

[simonschmeisser]

@gavanderhoorn how do we get started with the migration?

[gavanderhoorn]

@simonschmeisser wrote:

@gavanderhoorn how do we get started with the migration?

We'd need "someone" to extract all the required parameters and/or update/check @JeroenDM's work to extract them from the xacros for all the supported robot variants.

Then create pkgs out of those parameters and PR those.

[JeroenDM]

I have some time this weekend to work on the automatic extraction from the urdf / xacro files. But in the short term it is maybe faster to manually get the parameters from the robot and write a good test to check you work. (In any case I will do this for some robots to check the automatic conversion.)

[gavanderhoorn]

As much as I like extracting the parameters from urdfs -- as it would mean less work for users -- it might be best to actually do the work manually and hard-code the parameters in the various plugins / configurations.

We've had users change base xacros in the past (adding tool frames and links, changing tool0 and base fi) which broke the IKFast plugins in various ways (I would say: expected ways, but it wasn't obvious apparently).

As OPW has quite strict requirements on the kinematics of a manipulator for the algorithm to work, supporting automated extraction would seem to increase chances that users try to use any plugin(s) on their own xacros/urdfs, which will most likely fail.

From a maintenance / support perspective, a setup similar to the IKFast plugins (ie: hard-coded to match with one specific kinematic configuration) might actually be preferable.

[JeroenDM]

I see your point. I will focus the time I have on improving the moveit_opw_kinematics_plugin.

Just wondering, should the pull request for the kinematic parameters be done at the fanuc repository, or first at fanuc_experimental (and then when it is stable migrated to the first one)?
I guess here (aka fanuc) because we are discussing it here, but I'm not sure why.

[JeroenDM]

@gavanderhoorn I plan on doing the pull request for the kinematic parameter files on this repo, if that's ok with you?

[gavanderhoorn]

Can you describe a bit the way you want to approach this? New packages? A .yaml file in the respective robot support pkgs? Something else?

[JeroenDM]

I propose the following pull request:

WIP replace ikfast plugin with opw_kinematics for supported robots

• Update the kinematics.yaml files for the supported robots.
• I can leave the original yaml file under the name kinematics.yaml.old or kinematics.yaml.ikfast or something...

I will test this locally by creating a workspace where I add opw_kinematics and moveit_opw_kinematics_plugin. It is quit easy to quickly check it in rviz by launching the demo.launch for the robot. But I should somehow use the recently added tests to MoveIt!, or add similar tests to moveit_opw_kinematics_plugin.
The automated tests with travis will not pass I suppose..

By creating the pull request I can get feedback and comments of people who actually know the robots.

Or do you think it's better to first create a pull request for the opw plugin at moveit to get it integrated and properly tested before moving on in this repo?

[gavanderhoorn]

WIP replace ikfast plugin with opw_kinematics for supported robots

• Update the kinematics.yaml files for the supported robots.
• I can leave the original yaml file under the name kinematics.yaml.old or kinematics.yaml.ikfast or something...

So iiuc, opw_kinematics is actually MoveIt-agnostic, and your moveit_opw_kinematics_plugin wraps that into a MoveIt-compatible library.

Both plain opw_kinematics as well as moveit_opw_kinematics_plugin would require the same set of parameters describing the robot kinematics in a way that is compatible with the OPW algorithm.

Seeing as opw_kinematics itself does not need anything from MoveIt, my suggestion would be to not store the parameters in the config/kinematics.yaml of MoveIt packages, but to place them in the robot support packages (ie: fanuc_$series_support).

Users of plain opw_kinematics could then load them from the appropriate support package for their robot variant, and MoveIt configurations could be extended to load the same file -- in the correct namespace -- for moveit_opw_kinematics_plugin.

The robot support packages already have a mechanism (and a folder) for this: the config folder. It currently typically only contains a joint_names_$variant.yaml file, but we can place the OPW parameters in that same folder, say in a file called opw_kinematics_params_$variant.yaml.

I will test this locally by creating a workspace where I add opw_kinematics and moveit_opw_kinematics_plugin. It is quit easy to quickly check it in rviz by launching the demo.launch for the robot. But I should somehow use the recently added tests to MoveIt!, or add similar tests to moveit_opw_kinematics_plugin.
The automated tests with travis will not pass I suppose..

Testing would definitely be needed, and automated testing would be even better, but the IKFast plugins currently are also not tested, nor is there infrastructure to do that.

I would be ok with a (first) PR just adding the parameters. We can then do a manual test with a MoveIt configuration package we configure to use the moveit_opw_kinematics_plugin.

Or do you think it's better to first create a pull request for the opw plugin at moveit to get it integrated and properly tested before moving on in this repo?

Getting the package released -- whether as part of MoveIt or stand-alone for now -- would certainly makes things easier from a maintainer point-of-view. We could simply update the package manifests of the affected MoveIt packages and dependency resolution would do the right thing when running rosdep.

For a first test here with the robots in this repository it'd probably be fine to have to build it all from source though.

Releasing the plugin can then be done in parallel.

[simonschmeisser]

@gavanderhoorn right now kinematics.yaml is a "standalone" file and we can simply load the file from a robot specific (eg fanuc-experimental) generic (debian)package in the planning_context.launch of a site/project/installation "instance" package. Splitting the information in two files would require some sort of abstraction layer? An extra launch file?

Currently opw_kinematics actually has no parameter reading/loading code so maybe we could agree on having one kinematics.yaml (or kinematics_$variant.yaml) with all the info and descartes/other users of plain opw_kinematics could parse this file as well?

[gavanderhoorn]

@gavanderhoorn right now kinematics.yaml is a "standalone" file and we can simply load the file from a robot specific (eg fanuc-experimental) generic (debian)package in the planning_context.launch of a site/project/installation "instance" package. Splitting the information in two files would require some sort of abstraction layer? An extra launch file?

I'm not sure I completely understand: opw_kinematics requires certain parameters. That is independent of whether it is used -- via moveit_opw_kinematics_plugin -- in a MoveIt context or not.

I don't really like using kinematics.yaml for this, for three reasons:

1. if the file remains in the config dir of the MoveIt configs, I'd need the MoveIt config pkgs around if I'd want to use opw_kinematics -- or even moveit_opw_kinematics_plugin -- without MoveIt
2. if the file is relocated to the robot support packages, we'd be adding a "MoveIt file" to a MoveIt-agnostic package
3. kinematics.yaml can actually contain information on many more groups that may not be using opw_kinematics. It's also rather MoveIt configuration specific (ie: run MSA again and you'd have a new kinematics.yaml file if you added/removed/changed groups)

That's why I suggested hosting a new file in $robot_support_pkg/config with just the OPW parameters for each variant.

For use with MoveIt: add a rosparam load .. to your planning_context.launch or wherever you want to do it. As long as the parameters end-up in the correct namespace, everything should work.

For other use: rosparam load .. the file in some namespace and ros::param::get(..) everything.

Seems like the most flexible way to do this, no?

Currently opw_kinematics actually has no parameter reading/loading code so maybe we could agree on having one kinematics.yaml (or kinematics_$variant.yaml) with all the info and descartes/other users of plain opw_kinematics could parse this file as well?

I would say extending opw_kinematics with some kind of infrastructure to load its parameters from a .yaml file (or some other forms of configuration file) instead of hard-coding a struct would definitely be something to consider, yes.

But we don't necessarily absolutely require opw_kinematics to be able to do that right now for us to decide where to store our OPW parameters.

[gavanderhoorn]

Just as an example: I'm essentially suggesting we store these parameters in variant-specific files (from here):

kinematics_solver_geometric_parameters:
  a1:  0.025
  a2: -0.035
  b:   0.000
  c1:  0.400
  c2:  0.315
  c3:  0.365
  c4:  0.080
kinematics_solver_joint_offsets: [0.0, -1.57079632679, 0, 0, 0, 0]

I would probably not use those names though (as kinematics_solver_ is a prefix used by MoveIt for the parameters for the solvers).

[JeroenDM]

I would probably not use those names though (as kinematics_solver_ is a prefix used by MoveIt for the parameters for the solvers).

Do you suggest
a) changing the hard-coded names in the setOPWParameters function.
b) Adding the parameters and changing the names manually when using the plugin with MoveIt!.
c) Doing some loading and remapping in a launch file for a MoveIt! setup?

In any case I'm in favor of a MoveIt! independent setup as I (ironically) never use the opw_kinemtics plugin myself. (I do use MoveIt! for collision checking.) (And therefore, thank you for your feedback as a user, @simonschmeisser)
I'm also in favor of ease of use, but I trust that @gavanderhoorn has a lot of experience in that respect to make the right call.

Also, I probably won't have time the next two weekend to work on it as I'm going on a holiday. I have generated parameters for all supported fanuc robots already and tested some of them.

[gavanderhoorn]

I'll respond to the rest later, but:

@JeroenDM wrote:

I have generated parameters for all supported fanuc robots already and tested some of them.

could you make those available somewhere, so I can take a look?

[JeroenDM]

Certainly, but I don't have my work laptop with me today and they somehow didn't make it in to my online backup system. I hope tomorrow is ok.

[JeroenDM]

@gavanderhoorn https://github.com/JeroenDM/wip_fanuc_opw_parameters
As mentioned I only tested 3 of them, so the others could be completely wrong.
Although the the joint sign convention and offset seems to be the same for most of the robots. (Which I would expect them all being from one manufacturer.)

[gavanderhoorn]

Thanks. I'll take a look when I have some time.

---

Edit:

Although the the joint sign convention and offset seems to be the same for most of the robots. (Which I would expect them all being from one manufacturer.)

hah. I wouldn't be so sure :)

[simonschmeisser]

the second part using lookupParam looks like a good improvement in any case, maybe open a PR already?

I somewhat worry about having the group name in a third place besides srdf and kinematics.yaml but would otherwise be fine with this approach (but then again manipulator is almost like a constant ...)

[gavanderhoorn]

the second part using lookupParam looks like a good improvement in any case, maybe open a PR already?

Yes, I could do that. It's how lookupParam(..) is supposed to be used in any case.

I somewhat worry about having the group name in a third place besides srdf and kinematics.yaml but would otherwise be fine with this approach

I don't like it too much either, but:

• we can't embed the group name in the .yaml, as that would make the .yaml only work for groups with that exact name (not likely in user-created MoveIt configurations)
• the current kinematics.yaml (ie: the default one generated by MoveIt) already contains the group name as the main key

I'm starting to think the "make moveit_opw_kinematics_plugin load an additional yaml itself"-approach might not even be such a bad one the more I think about it.

but then again manipulator is almost like a constant ...

it's really just a convention we have in ros-i.

[simonschmeisser]

the 'loading a yaml file' approach could also be fairly generic and just load the file to the param server and then use the existing 'lookupParam' code

[gavanderhoorn]

I'm going to do an initial release into Melodic without the IKFast plugins, as I'd like to only release an OPW based set of packages.

Have you made any progress here @simonschmeisser? Or are you waiting on me to implement this?

[simonschmeisser]

My long term memory has this tagged as 'status unclear', have we decided on something? Will look into it tomorrow

[simonschmeisser]

so by implementing this we are talking about the following setup?

user_moveit_setup_package/config/kinematics.yaml:

manipulator:
  kinematics_solver: moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin
  opw_configuration_file: fanuc_m10ia_support:config/opw_configuration_m10ia7l.yaml

fanuc_m10ia_support:config/opw_configuration_m10ia7l.yaml

  geometric_parameters:
      a1:  0.05
      a2:  0.0
      b:   0.050
      c1:  0.478
      c2:  0.500
      c3:  0.550
      c4:  0.100
  joint_offsets: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  joint_sign_corrections: [1, 1, 1, 1, 1, 1]

(I removed the kinematics_solver_ prefix)

and then the plugin loads the opw_configuration_file onto the parameter server before reading the params from there

I can implement this during the week

---

I just thought it would be cool if pluginlib had a way to put arguments into the xml file and pass them to the plugin, then this could be completely transparent to moveit setup assistent

http://wiki.ros.org/pluginlib/PluginDescriptionFile

[gavanderhoorn]

My apologies: I seemed to remember we wanted to test some alternatives before deciding on a particular way forward.

so by implementing this we are talking about the following setup?

Not necessarily the part that loads a .yaml file for the additional parameters, but the general OPW kinematics plugin compatibility.

re: loading the .yaml: in principle it's not a bad idea, but it diverges from how ROS parameters are normally populated. It will also -- I assume -- load the parameters in a namespace which the OPW plugin determines. This would take control over where parameters end up away from users.

I just thought it would be cool if pluginlib had a way to put arguments into the xml file and pass them to the plugin, then this could be completely transparent to moveit setup assistent

http://wiki.ros.org/pluginlib/PluginDescriptionFile

hm. With 'parameters' you mean the path to the .yaml, or the actual parameters?

How would you configure a particular group to use a particular plugin + set of parameters in that case?

[simonschmeisser]

I just thought it would be cool if pluginlib had a way to put arguments into the xml file and pass them to the plugin, then this could be completely transparent to moveit setup assistent
http://wiki.ros.org/pluginlib/PluginDescriptionFile

hm. With 'parameters' you mean the path to the .yaml, or the actual parameters?

How would you configure a particular group to use a particular plugin + set of parameters in that case?

Should have elaborated a bit more, my idea was to have a model specific xml file containing the path to the specific yaml file and then you would just load this in kinematics.yaml/select it in moveit setup assistant

eg

manipulator:
  kinematics_solver: fanuc_m10ia_moveit_plugin/M10ia7lMoveItOPWKinematicsPlugin

[gavanderhoorn]

eg

manipulator:
  kinematics_solver: fanuc_m10ia_moveit_plugin/M10ia7lMoveItOPWKinematicsPlugin

Would the plugin.xml then contain a 'fake' name, and each 'instance' would point to the same moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin, which would then be passed the parameters by pluginlib?

That would require plugin.xmls for each MoveItOPWKinematicsPlugin wrapper, but could work.

But would we then host those plugin.xml files in the MoveIt configuration packages? They're not really MoveIt specific, are they?

If we'd have to create packages to host those files in that would be quite some overhead (essentially replacing IKFast packages with OPW Kinematics ones).

[simonschmeisser]

Would the plugin.xml then contain a 'fake' name, and each 'instance' would point to the same moveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin, which would then be passed the parameters by pluginlib?

exactly

That would require plugin.xmls for each MoveItOPWKinematicsPlugin wrapper, but could work.

but it would be plug 'n play with current moveit setup assistant and co

But would we then host those plugin.xml files in the MoveIt configuration packages? They're not really MoveIt specific, are they?

If we'd have to create packages to host those files in that would be quite some overhead (essentially replacing IKFast packages with OPW Kinematics ones).

I would be fine with/prefer having those plugin.xml files in the _support package but I fear this all fails due to not being able to pass data (ie the path to the yaml) from xml to the plugin instance?

[gavanderhoorn]

I fear this all fails due to not being able to pass data (ie the path to the yaml) from xml to the plugin instance?

yes, there is no support for that indeed.

But I like the idea of giving different names to the same type of plugin. That may be something to look into.

[gavanderhoorn]

Just did a quick test with ros-planning/moveit@master and generated an IKFast plugin for the M-10iA.

Just the solver itself increased in size from ~150KB to ~550KB. Not sure what changed in IKFast, but there wasn't a measurable performance increase or better IK results.

I believe the plan to migrate everything over to OPW makes sense from this alone.

[simonschmeisser]

550KB of generated code? Sounds hard to sell given that opw is 270 lines of code ... also I might be mistaken but I think moveit_opw_kinematics_plugin handles special cases like seeds outside +-180° better by now

I'm a bit afraid we're running circles here trying to find a solution that

• does not require additional launch files / changes to launch files
• is compatible with moveit_assistant out of the box
• separates opw parameters from MoveIt kinematics.yaml for Descartes etc
• does not involve magic like guessing the location of opw parameter files based on urdf/mesh??? files
• does not side-load parameter files (see Migrate to opw_kinematics for all supported robots #245 (comment))

I would personally go for one of

• adding the kinematics.yaml to the support package in MoveIt format (and extending moveit_setup_assistant to be able to load external kinematics files instead of creating new ones)
• adding a parameter to load a separate yaml file as in Migrate to opw_kinematics for all supported robots #245 (comment) , could be overwritten by parameter set in the conventional way (or error out if both are present)
• combination of both :)

But I'd happy to implement/support anything else as well :)

[gavanderhoorn]

550KB of generated code?

Yes. For this particular robot that's an increase of about 400KB. Assuming similar increases for other models and variants, that would lead to a 25MB increase just to update the IKFast plugins.

Sounds hard to sell given that opw is 270 lines of code ... also I might be mistaken but I think moveit_opw_kinematics_plugin handles special cases like seeds outside +-180° better by now

Exactly. All the more reason to use OPW.

As for the rest: MSA compatibility for many other IK plugins essentially comes down to being able to select it from the dropdown. Any solver-specific parameters need to be added/loaded by hand.

---

As to the rest: someone will need to come up with something acceptable and provide an implementation. Only then can we start evaluating the pros and cons.

[simonschmeisser]

I have another idea I would like to throw in here:
While we seem to agree that deriving the parameters directly from URDF might be too difficult/impossible/error-prone ... we could still try to embed the OPW parameters into the URDF. There is support for loading YAML files from xacro, so we could put the parameters in a generic file in config and load this from _macro.xacro. It could then be added as a parameter/child nodes of tool_0 link. The MoveIt kinematics.yaml could then be completely generic again and other consumers like Descartes could either read the yaml/rosparam or be extended to parse the robot_description/urdf as well

Do you think this idea is worth a prototype?

[gavanderhoorn]

hm. That may actually be an idea.

Not sure about attaching them to tool0 necessarily. Can you explain why you'd want to use that link specifically?

[simonschmeisser]

Not sure about attaching them to tool0 necessarily. Can you explain why you'd want to use that link specifically?

The plugin gets the following information: robot_model, group_name, base_frame and tip_frame(s)

group_name is not part of the URDF but rather SRDF which typically is not composed/composable

so we basically have the choice between base_frame and tip_frame (eg. tool0) if we want to attach the information to some uniquely identifiable xml node even in multi-robot setups. I was under the impression that there might be cases where base_frame could be shared between multiple kinematic chains (think SDA20D or yumi but 6 DoF). Therefore I chose tool0.

Note that with this scheme we still need to guess/default value /robot_description

[JeroenDM]

I went ahead and create the pull request (#284 ) to add the parameters to the support packages.

In going forward, it appears to me that we have three design goals.

• Make the parameters available in a generic config file for non-MoveIt use.
• Make it easy for users to run the setup assistant to create a MoveIt config. Ideally, just select the opw_kinematics_plugin where the setup assistant figures out the details.
• Keep everything easy to maintain.

The pull request addresses the first point.

[gavanderhoorn]

With #284 merged, we're finally making some progress here.

Thanks @simonschmeisser and @JeroenDM for pushing this forward.

[JeroenDM]

I was kind of surprised/embarrassed to see when this issue was opened (aka when I started working on the plugin). I did not work particularly fast.

I'm happy to see to some progress too :)

[gavanderhoorn]

moveit_opw_kinematics_plugin has been synced to the public repositories, so we can start using it in released packages as well.

Thanks again @JeroenDM for the work 👍

We'll keep this open until all support packages have OPW parameters and we've switched the MoveIt configurations to use the OPW plugin.

[bourumir-wyngs]

This for sure will be amazing. I am working on experimental automated OPW parameter extraction from URDF. The biggest problem so far, we have not enough robots with both verified URDF and verified OPW parameters to build a descent testing suite.

[simonschmeisser]

You can find more examples in kuka-experimental, abb, doosan ... Im some cases only in our isys-vision forks

Also the self check should tell you if it works ( comparig forward kinematics via urdf and via opw parameters for two poses)