Add support for constants/symbolic values (offsets, geometric parameters)

[gavanderhoorn]

Originally from https://github.com/ros-industrial/fanuc/commit/8e668977a0a4f0f5d2fcaede2bb3af1392b0daeb#r41337106.

In addition to specifying values for the joint_offsets and geometric_parameters parameters with plain doubles, it would perhaps be an idea to also support symbols or named constants, such as pi.

This could avoid situations where the number of significant digits (negatively) influences the accuracy of the calculations in the OPW plugin.

[gavanderhoorn]

One way to implement this would be to only support names such as pi and half_pi (or m_pi_2 or similar).

That would only require some ifs and elses.

Taking it a step further and using a proper expression parser would allow supporting things like: pi/2 and other arithmetic expressions.

muparser could be used for this. It's relatively low-overhead and also used by ros_canopen for its unit conversion functionality (together with Boost).

[JeroenDM]

Thanks for creating the issue and adding the useful reference.

I'm not going to work on it in the near future though, maybe after my GSoC project. (It might also be a good beginner issue for someone looking to get started in ros dev.)

[gavanderhoorn]

So I just spent some time on this, and then noticed this on the rosparam wiki page:

There are also special converters for angle radian/degree representations. Any Python-legal mathematical expression can be used with the radian value, with pi used to represent pi.

So instead of:

opw_kinematics_joint_offsets: [0.0, 0.0, -1.57079632679, 0.0, 0.0, 3.14159265359]

you can actually just do:

opw_kinematics_joint_offsets: [0.0, 0.0, rad(-pi/2), 0.0, 0.0, rad(pi)]

which results in exactly what we wanted, without having to do anything special in moveit_opw_kinematics_plugin. The limitation to precision here would be whatever Python uses, and how this is treated by the parameter server.

I've used this before, but just never thought of it in this context.

Output of moveit_opw_kinematics_plugin on the console in both cases (limited nr of significant digits is due to settings in rosconsole):

[ INFO] [1597312860.858675980] [..] [ros.moveit_opw_kinematics_plugin]: Loaded parameters for ik solver:
Distances: [0.15 -0.2 0 0.45 0.6 0.64 0.1]
Offsets = [0 0 -1.5708 0 0 3.14159 ]
Sign_corrections = [1 1 -1 -1 -1 -1 ]

Something to think about of course would be that this increases "coupling": for consumers of the .yaml file without support for this special form of parsing, loading the parameters will most likely fail.

[gavanderhoorn]

As this seems like a no-op now, I'm closing this.

[gavanderhoorn]

@gavanderhoorn wrote:

The limitation to precision here would be whatever Python uses, and how this is treated by the parameter server.

Just to check:

$ rosparam get /robot_description_kinematics/opw_kinematics_joint_offsets 
[0.0, 0.0, -1.5707963267948966, 0.0, 0.0, 3.141592653589793]

[gavanderhoorn]

Seeing as this also supports deg(180) and converts it to radians: @JeroenDM @simonschmeisser: what would you feel would be "nicer"? Speccing these offsets in degrees, or in radians?

Degrees are easier to understand / mentally picture for humans:

opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90), 0.0, 0.0, deg(180)]

this would still result in

opw_kinematics_joint_offsets: [0.0, 0.0, -1.5707963267948966, 0.0, 0.0, 3.141592653589793]

on the parameter server

---

Edit: I've gone with degrees: https://github.com/ros-industrial/fanuc/pull/293.

[JeroenDM]

Had I known it existed I would have used it from the beginning! Especially the deg(180) seems very readable to me. I suppose it is also compatible with the moveit_setup_assistant tool.

Something to think about of course would be that this increases "coupling": for consumers of the .yaml file without support for this special form of parsing, loading the parameters will most likely fail.

That is indeed less ideal, but I think writing a script to convert the file before parsing is not that difficult and therefore a reasonable expectation for non-ros users of this data.

[simonschmeisser]

wow, that's a really nice feature I didn't know about!

I'm a bit confused because in XACRO we write radians(90) and here deg(90)?

[gavanderhoorn]

@JeroenDM wrote:

I suppose it is also compatible with the moveit_setup_assistant tool.

Anything which uses rosparam (either the stand-alone tool, the support in roslaunch or library) should support this.

xacro doesn't (yet). I've opened ros/xacro#251 to discuss adding it, as I'd like to use this in other files for other robots as well (such as https://github.com/ros-industrial/universal_robot/compare/melodic-devel-staging...gavanderhoorn:yaml_ctors_rosparam_joint_limits).

[gavanderhoorn]

@simonschmeisser wrote:

I'm a bit confused because in XACRO we write radians(90) and here deg(90)?

Yes, that's because xacro doesn't use rosparam's parsing for this.

See the issue I opened in ros/xacro.

---

Edit: oh, I believe you're actually confused about the fact we write radians(..) in xacro, which does degree->radians conversion, but then write deg(..) in rosparam, which does degree->radians conversion.

It's just personal thing I guess. rosparam developers use the name to declare what it is you're passing in, and the conversion to radians is 'implicit' almost (everything converts to radians). In xacro, you can just use radians(..), which takes in degrees and outputs radians. There is nothing else.

rosparam also uses PyYAML constructors for this.