the C++ quaternion/rpy conversion has numerical issues

[sbarthelemy]

two very similar quaternions can result in different and non-equivalent roll pitch yaw angles.

Unint test showing the issue coming soon.

[sbarthelemy]

So, the urdf::Rotation internally stores rotations as quaternions, and has the following methods:

• getQuaternion
• setFromQuaternion
• getRPY
• setFromRPY

Given that the URDF xml represents rotations as RPY angles, I wonder why urdf::Rotation does not uses RPY internally too.

1/ Shall I propose a patch which changes that? (not really needed)

Then, regarding the RPY/quaternion problem, we could probably fix it by removing the custom code and using Eigen (see [a] and [b])

This could be done by either

2/ replacing urdf::Rotation with the equivalent Eigen class (with a typedef or so), or

3/ keeping the present interface but useing Eigen as a backend (so Eigen would be a build dep but not a runtime dep)

what do you think?

[a] http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gad118fececd448d7485ffea4858775e5a
[b] http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html

[traversaro]

My 2 cents: if we really want to add Eigen as dependency, I'll strongly suggest going for option (3): Eigen is an heavily template library and for properly exporting an Eigen datatype it in the shared library interface the compiler have to generate all the code relative to the used instantiation of the template class, resulting in large binaries.

[traversaro]

An alternative to the use of Eigen could be porting some relevant code to the urdf::Rotation class (Gazebo for example has a similar class [1], but I don't know if it is affected by the same numerical problem). This leads to a bit of code duplication, but does not add any new dependency to urdfdom.

[1] : http://ignitionrobotics.org/libraries/ign_math/api/classignition_1_1math_1_1Quaternion.html

[vrabaud]

@hsu , it seems that code comes from Gazebo that has the same problem:
https://bitbucket.org/osrf/gazebo/src/66de1887edea28bec5d8b94de5e74f035ac53cc0/gazebo/math/Quaternion.cc?at=default#cl-257

[scpeters]

Gazebo developer here; @j-rivero and I will look into adding that unit test to the gazebo math library.

We've thought about eigen but don't have any short-term plans to incorporate it into the default gazebo math library. Gazebo can optionally link with the dartsim physics engine, but that's currently the only connection with eigen that I know of.

[j-rivero]

By the way, gazebo6 has split out the math layer and create a common library for it, called ignition robotics math. Eventually, I agree with @traversaro, it would be nice to use Eigen as a backend.

[scpeters]

fixed by ros/urdfdom_headers#11