Open gavanderhoorn opened 6 years ago
just came across one "small" issue ... https://github.com/Jmeyer1292/opw_kinematics/issues/11
Hm. I had not noticed that.
Let's see what @Jmeyer1292's response is.
@Jmeyer1292 changed the license to Apache 2 so licensing would no longer seem to be an issue.
the MoveIt! plugin by @jeroenDM is Apache since yesterday as well and a first quick test worked quite nicely
Did you observe the "low performance" that @JeroenDM was complaining about some time ago?
@gavanderhoorn it "feels sane" but I'm waiting for https://github.com/ros-planning/moveit/pull/1272 to actually measure it. I have some other unrelated but huge performance issue that I need to profile first.
@gavanderhoorn how do we get started with the migration?
@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.
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.)
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.
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.
@gavanderhoorn I plan on doing the pull request for the kinematic parameter files on this repo, if that's ok with you?
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?
I propose the following pull request:
kinematics.yaml
files for the supported robots.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?
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
orkinematics.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
andmoveit_opw_kinematics_plugin
. It is quit easy to quickly check it in rviz by launching thedemo.launch
for the robot. But I should somehow use the recently added tests to MoveIt!, or add similar tests tomoveit_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.
@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 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:
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 MoveItkinematics.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 onekinematics.yaml
(orkinematics_$variant.yaml
) with all the info anddescartes
/other users of plainopw_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.
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).
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.
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?
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.
@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.)
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 :)
So I had some time to play with this and first and foremost: nice job @JeroenDM.
As to parameter naming, loading and storage: I've gone ahead and tried the minimal amount of changes needed to use generic parameter names while still allow MoveIt usage.
See diff for the changes to the M-10iA support and MoveIt cfg pkg and diff for the changes to moveit_opw_kinematics_plugin
.
While this keeps OPW parameters generic (ie: not tied to MoveIt), I'm not too thrilled about it. It feels a bit convoluted to have to add the additional rosparam .. load
there with the ns
set to the name of the group for which the parameters are loaded.
I've thought about using parameter remapping, but that would require more (invasive) changes.
@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 believe I now understand what you were referring to @simonschmeisser. However thinking about it I don't believe we should introduce an extra launch file: MoveIt requires a configuration item called kinematics_solver
(which could be in the group namespace or somewhere else). That solver may require certain parameters. But there is no requirement for those parameters to exist anywhere specifically, they could be loaded from a file. As long as the solver knows how to find them, that should be enough.
Loading the parameters in the kinematics.yaml
file could be an option, but would also require the MoveIt configuration pkg to be present on a system that would want to load those parameters. That would lead to MoveIt being installed on systems that may not even want/need it (as MoveIt configuration pkgs hard-depend on MoveIt itself).
Following this logic config/kinematics.yaml
now only contains the kinematics_solver
key. A new .yaml
file in the robot's support pkg contains the parameters. A MoveIt configuration -- if it is configured to use OPW -- must load the parameters into the correct namespace. That can be done in planning_context.launch
with a simple rosparam load
.
As I wrote above: I'm not entirely happy (yet) about this, as it requires additional manual changes to kinematics.yaml
and two .launch
files per MoveIt config pkg, but using moveit_opw_kinematics_plugin
(without splitting things like this) would require that anyway.
@Levi-Armstrong: FYI.
@gavanderhoorn How can we move forward here? We are getting closer to having some deployments and stabilizing the packaging would be nice for future updates :)
I would say that personally I'd like to store any parameters related to the various robots in their respective support packages. I'd like to avoid having joint limits in the support packages and then the OPW parameters in a moveit config. It's all just physical properties of the robots we support and those should be hosted in their support packages.
As I wrote above: it's perfectly reasonable to want to use the OPW kinematics plugin without MoveIt, but storing the parameters in the MoveIt configuration makes that essentially impossible (if we assume the typical installation scenario with read-only, deb
distributed packages).
https://github.com/ros-industrial/fanuc/compare/indigo-devel...gavanderhoorn:m10_opw_kinem shows the changes on the fanuc
side. The only thing I don't like too much is adding additional lines to Setup Assistant generates files, but I don't see a way around it.
https://github.com/JeroenDM/moveit_opw_kinematics_plugin/compare/kinetic-devel...gavanderhoorn:generic_params has three changes that would have to go into moveit_opw_kinematics_plugin
. As they're only name-changes, I would be ok with foregoing them and just sticking with kinematics_solver_
prefixes for everything (which is really MoveIt 'vernacular'), instead of the more generic opw_kinematics_
.
So to move forward:
moveit_opw_kinematics_plugin
or just stick with the current names of the parametersIt's unfortunate there is no "include system" in yaml, as that would immediately solve this: the MoveIt kinematics.yaml
could just include the OPW parameters yaml and nothing would have to change.
moveit_setup_assistant's handling of kinematics plugins seems to be very basic ... https://github.com/ros-planning/moveit/blob/97edff950f389cc9c7880ed2e7ba4b3a652e3539/moveit_setup_assistant/src/widgets/group_edit_widget.cpp#L285
As we have our own more end-user oriented assistant I don't mind changing the generated files now. We would probably need to add some very clear error message (link?) on how to load the parameter if not found and add opw to the moveit tutorials and it should be fine
What do you think @JeroenDM ? Are there other users yet?
If we don't change the names it would even be backwards compatible right? (ie. moveit_opw_kinematics_plugin
would need no changes)
As we have our own more end-user oriented assistant I don't mind changing the generated files now.
No, maybe you don't, but I'd have to update 18 moveit configuration pkgs and make sure those changes do not disappear whenever updating them with the MSA.
We would probably need to add some very clear error message (link?) on how to load the parameter if not found and add opw to the moveit tutorials and it should be fine
I'm not sure I understand completely: the OPW plugin could just error-out when it can't find the required parameters. A red ERROR
for each missing parameter and then returning a failure on each FK/IK query would probably get the message across quite well.
Or are you thinking of users creating their own MoveIt configurations for Fanuc robots and then selecting the OPW plugin? They will probably not have a problem with embedding the parameters in the kinematics.yaml
file.
It's going to be a bit nasty in any case, as the parameters must be loaded in the groups namespace .. (like this).
So for every group where you'd want to use OPW you'd have to add a rosparam
there :unamused:.
If we don't change the names it would even be backwards compatible right? (ie.
moveit_opw_kinematics_plugin
would need no changes)
Correct.
I could live with not changing them. But I do feel it's clearer with the opw_kinematics_
prefix, as that's what the parameters are for. kinematics_solver_
is rather generic.
it's not a very common approach but such an include for yaml could be implemented in the moveit_opw_kinematics_plugin
as well. ie have a param 'opw_param_file' in kinematics.yaml and load this either directly or to the param server.
re: errors: we could add a link to some docs, but the errors are quite clear already:
[ERROR] [1562962920.661951283] [/move_group] [ros.moveit_opw_kinematics_plugin]: Failed to load geometric parameters for ik solver.
[ERROR] [1562962920.661999615] [/move_group] [ros.moveit_opw_kinematics_plugin.opw]: Could not load opw parameters. Check kinematics.yaml.
[ERROR] [1562962920.662016003] [/move_group] [ros.moveit_ros_planning]: Kinematics solver of type 'moveit_opw_kinematics_plugin/MoveItOPWKinematicsPlugin' could not be initialized for group 'manipulator'
[ERROR] [1562962920.662173087] [/move_group] [ros.moveit_ros_planning]: Kinematics solver could not be instantiated for joint group manipulator.
Ok. A slightly better version:
planning_context.launch
(https://github.com/ros-industrial/fanuc/compare/indigo-devel...gavanderhoorn:m10_opw_kinem)lookupParam(..)
actually search for the parameters, instead of hard-coding the path to absolute parameters (https://github.com/isys-vision/moveit_opw_kinematics_plugin/compare/opw-pure-cmake...gavanderhoorn:no_param_prefix)This way we only need to add 1 rosparam
per group, and it will be loaded by all .launch
files that (ultimately) include planning_context.launch
(ie: demo.launch
and moveit_planning_execution.launch
).
I'm still not too happy about the "one rosparam
per group" (as that will lead to questions from users), but apart from extending moveit_opw_kinematics_plugin
to load a .yaml
file itself or just embedding all parameters in kinematics.yaml
I can't really think of something less invasive.
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 ...)
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:
.yaml
, as that would make the .yaml
only work for groups with that exact name (not likely in user-created MoveIt configurations)kinematics.yaml
(ie: the default one generated by MoveIt) already contains the group name as the main keyI'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.
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
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?
My long term memory has this tagged as 'status unclear', have we decided on something? Will look into it tomorrow
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
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
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?
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
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.xml
s 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).
Would the
plugin.xml
then contain a 'fake' name, and each 'instance' would point to the samemoveit_opw_kinematics_plugin::MoveItOPWKinematicsPlugin
, which would then be passed the parameters bypluginlib
?exactly
That would require
plugin.xml
s for eachMoveItOPWKinematicsPlugin
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?
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.
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.
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
I would personally go for one of
But I'd happy to implement/support anything else as well :)
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.
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?
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?
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
Instead of IKFast use Jmeyer1292/opw_kinematics for all robot models and variants we support.
Current status: