Add quaternion in URDF attributes

[VictorLamoine]

Fixes #13

⚠️ Depends on ros/urdfdom_headers#51

Short description

Only rpy rotations are supported in urdf now. This pull request allows to use quaternions inside of urdf files.

URDF syntax example

<origin quat_xyzw="0 0 0.7071 0.7071" xyz="2.5 0 0"/>

How to test

Compile / install modified urdfdom_headers

mkdir -p ~/libraries/urdfdom_headers/build
cd ~/libraries/urdfdom_headers
git clone https://github.com/VictorLamoine/urdfdom_headers.git src
cd build
cmake ../src
sudo make -j4 install

Compile / install modified urdfdom

mkdir -p ~/libraries/urdfdom/build
cd ~/libraries/urdfdom
git clone https://github.com/VictorLamoine/urdfdom.git src
cd build
cmake ../src
sudo make -j4 install

Change LD_LIBRARY_PATH to make sure new urdfdom version will be used

LD_LIBRARY_PATH=/usr/local/lib/:$LD_LIBRARY_PATH 

Compile a simple example package and visualize result in RViz

mkdir -p ~/urdfdom_test_ws/src
cd ~/urdfdom_test_ws/src
git clone --branch urdfdom_quaternions https://gitlab.com/InstitutMaupertuis/test_robot.git
cd ..
catkin_make
source devel/setup.bash
roslaunch test_robot robot.launch

Green suzanne is turned 90° around the Z axis because the provided quaternion is 0 0 0.7071 0.7071 (xyzw):

Notes

• The order is xyzw, some libraries (Eigen) initialize in the wxyz order, so we must document the initialization order.
• Providing a quaternion with less or more than 4 values will lead to an error.
• Providing a non normalized quaternion leads to undefined behavior but the quaternion is normalized after being parsed. This should avoid nasty errors.
• Providing both euler angles and a quaternion will lead to an error.

[clalancette]

With the one exception of the name of the method, this looks pretty good to me.

[traversaro]

URDF is quite used also outside of ROS/urdfdom, including in several commercial and proprietary products. A change like this renders all the existing URDF parser implementations incomplete, and can create confusion in the users that do not understand why a URDF that is correctly parsed by urdfdom is not parsed by a third party tool. Perhaps it would make to add some kind of versioning to the format, so that the quaternion-less URDF would be "URDFv1", and the new URDF would be "URDFv2", and libraries that needs to be updated could simply claim compatibility with "URDFv1" (and incompatibility with "URDFv2")?

[traversaro]

In the current implementation, if both rpy and quat are defined, the quaternion is silently used, and no error is generated. Should this be documented as the intended behaviour also for third-party parsers?

[clalancette]

URDF is quite used also outside of ROS/urdfdom, including in several commercial and proprietary products. A change like this renders all the existing URDF parser implementations incomplete, and can create confusion in the users that do not understand why a URDF that is correctly parsed by urdfdom is not parsed by a third party tool. Perhaps it would make to add some kind of versioning to the format, so that the quaternion-less URDF would be "URDFv1", and the new URDF would be "URDFv2", and libraries that needs to be updated could simply claim compatibility with "URDFv1" (and incompatibility with "URDFv2")?

I think having a version in URDF is a good thing in general.

And if this change broke existing URDF, then I'd definitely be on board with versioning it and being sure that other implementations knew about it. But for backwards compatible changes like this one, I have to argue that we must be able to iterate on it. Otherwise, the format becomes set in stone and eventually becomes irrelevant as the world changes around it. Also, bumping the URDF version every time we add a new field just doesn't seem sustainable.

In the current implementation, if both rpy and quat are defined, the quaternion is silently used, and no error is generated. Should this be documented as the intended behaviour also for third-party parsers?

That's a really good point. I'd actually argue that we should make the parser throw an error at that point, since it is unclear what the user means. I'll add a note to the implementation.

[traversaro]

And if this change broke existing URDF, then I'd definitely be on board with versioning it and being sure that other implementations knew about it. But for backwards compatible changes like this one, I have to argue that we must be able to iterate on it.

I agree that this change is backward compatible from the point of view of who genererates models: your old model will continue to be loaded as before in the new urdfdom. However, it is not backward compatible for authors of library, tools and products that consume URDFs: as soon as it is merged in master, there will be URDFs that will be accepted by urdfdom, while being silently parsed in the wrong way in all other libraries that consume URDF. The tricky part is the currently if the <rpy ...> element is not there (see http://wiki.ros.org/urdf/XML/joint), it is assumed that the corresponding orientation is 0.0 0.0 0.0: so if a URDF with <quat ..> element is passed to a library expecting an "old", it will not complain, as it is perfectly legal not to have a rpy element, and the identity rotation will be using, ignoring the <quat ..> element.

Also, bumping the URDF version every time we add a new field just doesn't seem sustainable.

Note that there is no need to bump the URDF major version, we could have a minor/patch version and just bump that one.

Do not get my wrong: I totally agree that we should have space to evolve the URDF format: I just think we should try to do that limiting as much as possible the confusion for users of the format itself.

[VictorLamoine]

I have modified the code so that an error is thrown if both rpy and quat are specified.

<origin quat_xyzw="0 0 0.7071 0.7071" rpy="0 0 0" xyz="2.5 0 0"/>

Will lead to:

[ERROR] [1551279485.356898376]: Both rpy and quat_xyzw orientations are defined. Use either one or the other.
[ERROR] [1551279485.356938194]: Could not parse visual element for Link [suzanne_green]

[eacousineau]

Is there somewhere that it can be explicitly documented what the order of the quaternion is, in a way such that people don't have to look at the documentation of some upstream math library?

(This looks like xyzw... as a minor side comment, perhaps wxyz - where the non-imaginary part comes first - is a way to up-front distinguish the name from translation, if you were to make some accessor like wxyz or what not.)

[clalancette]

(This looks like xyzw... as a minor side comment, perhaps wxyz - where the non-imaginary part comes first - is a way to up-front distinguish the name from translation, if you were to make some accessor like wxyz or what not.)

I highly prefer xyzw, since that is what most of the ROS stuff uses (going to and from Eigen is the place where this hurts, but at least we should be internally consistent).

[eacousineau]

Sounds good!

[eacousineau]

If it's not too late, can I make a plea to make the ordering of quaternions explicit in the name? Something like quat_wxyz / q_wxyz vs. quat_xyzw / q_xyzw or something like that?

[clalancette]

If it's not too late, can I make a plea to make the ordering of quaternions explicit in the name? Something like quat_wxyz / q_wxyz vs. quat_xyzw / q_xyzw or something like that?

That's a good idea, I'm good with that. I'd go for quat_xyzw.

[VictorLamoine]

Good idea, explicit is better than implicit.
I've done the modification.

[eacousineau]

Sweet! Thanks!

[VictorLamoine]

Any other ideas or remarks?
Is this PR good for merging or are we waiting for another review?

[clalancette]

Is this PR good for merging or are we waiting for another review?

I'm not the official maintainer of urdfdom, so I'd like to get thoughts/reviews from @scpeters and/or @sloretz before we merge.

[scpeters]

I agree with @traversaro that we some kind of versioning for URDF as features are added. There is a version attribute in urdf.xsd, so we should probably start using it for new features.

[eacousineau]

I actually do have one question: What's the motivation for adding this format? What workflow is improved by having this?
#13 makes a request, but doesn't really indicate why quaternions are actually nice, and what the acute use case is.
(TBH, I'd probably not make any use of this feature unless I ran into (b) / (c).)

(a) Is it easier for you to interpret the physical meaning of a quaternion instead of rpy?
(b) Is it for using tools that generate URDFs?
(c) Is there some sort of loss of precision that occurs when trying to convert quaternions that you use in your toolset to URDF rpy? (kind of related to (b)...)

I ask because I've filed #124 motivated by (a) being able to better physically interpret the meaning of numbers. (Even if I can recognize 1.57... and what not and kind-of guess the offset, it's a whole lot easier for me to recognize 90.)

[VictorLamoine]

I will change the version of the urdfdom project and XML version attribute

---

Angles are represented with quaternions within ROS, pretty much everywhere. So it makes sense to be able to use/get quaternions everywhere.
I encourage you to look at https://www.youtube.com/watch?v=d4EgbgTm0Bg

(a) Yes, unlike Euler angles there is no conventions with quaternion, getting the rotation matrix from quaternions is straightforward, with Euler angles you have to make sure of what convention is used; if you use the wrong one, you get a wrong result.
(b) Being able to write URDF files in quaternions means not converting 3 times my rotation matrix to get the right results, which is easier, takes less time and is less error prone.
(c) Yes but that is negligible in everything that I have used

I ask because I've filed #124 motivated by (a) being able to better physically interpret the meaning of numbers. (Even if I can recognize 1.57... and what not and kind-of guess the offset, it's a whole lot easier for me to recognize 90.)

If you train your brain to work in radians you will be able to play with them like with degrees, but we are usually taught to think with degrees 😄.

[eacousineau]

That all makes sense, thanks for explaining!

For (a) and (b), that can be partially resolved by the standard explicitly stating what type of Euler angles there are, rather than trying to cross-ref. software and heuristically match the representations.

For the degrees bit, I have trained myself to think in radians, but I still hate it :P I can certainly interpret radians, but I do prefer the simplicity and "roundness" of degrees, and if modeling formats tend towards human readability, RPY in degrees are a little bit more quickly interpreted for me (and require fewer duplicate computations, e.g. storing / computing pi_4, pi_2, etc.).

Thanks!

[VictorLamoine]

Are you ok with the changes? What is preventing this pull request from being merged?

[eacousineau]

I think this may be dependent on what @scpeters said about versioning?

I agree with @traversaro that we some kind of versioning for URDF as features are added.

[VictorLamoine]

Ping, I've updated the file versions number so we should be good.

[traversaro]

I am personally totally fine with using the version attribute of URDF.
However, I think that the attribute present in the XSD is actually just a suggestion by TM (that I guess indicates @thomas-moulard), and not something that have been ever used in practice:
b377abc#diff-f78a0a18b2f5a8d7456fc33a7849e139L321

If we start to use it, I suggest to remove the TM comment from the XSD, and update the URDF wiki page http://wiki.ros.org/urdf that in my experience is de facto "specification" of what an URDF is (see for example #16, where the XSD is fixed because it does not agree with the wiki page).

After that, it will be up to the URDF consuming/producing software to correctly parse or reject URDF v1.1 files. However, I wonder what strategy should be used for ROS packages that parse URDFs, but without using urdfdom, such as https://github.com/ros/urdf_parser_py . They should be updated to handle v1.1 URDFs?

[MarqRazz]

While this PR is open I wonder if we could also talk about adding support for custom transforms to be defined in the URDF (allow the user to supply the full 4x4 matrix).

My use case is that I am working on supporting robots by utilizing the DH convention. I would like to make a single URDF that takes in a DH parameter table and produces a robot_description parameter in ROS. This would allow me to take calibration parameters given by industrial robots (I.E. The Yaskawa Motoman offers the ability to query its calibrated DH parameters) and read/apply them on the next startup without hand modifying the URDF. I have a simple version of this working with the current framework, only works if the theta_offset of the DH is zero, because the two rotations that are applied in DH are not done in the same order that URDF applies them. I could get a system fully working if I made dummy links between links that applied the rotation properly, or recalculated the transform to the ROS convention with another program but would really like to avoid these approaches if possible.

For example here is the forward kinematics code I made in school to apply a single DH transform...

% dhtrans   Calculates the homogeneous transformation matrix
%  
%	function T = dhtrans(theta,alpha,a,d)
%	Computes homogeneous transformation matrix given DH parameters 
%   for a particular link
%  
%	T =  Homogeneous transformation from frame i to frame i-1
%
%   theta = angle about z_i-1 from x_i-1 to x_i
%   alpha = angle about x_i from z_i-1 to z_i
%	a = distance along x_i from z_i-1 to z_i
%   d = distance along z_i-1 from x_i-1 to x_i
%
%	Marques Rasmussen
%	ME 6220
%	10-05-08

function T = dhtrans(theta,alpha,a,d)

Ct = cos(theta);
St = sin(theta);
Ca = cos(alpha);
Sa = sin(alpha);

%For derivation of the rotation matrix see section 4.6 equation 4.2
%Could have also use functions rotz(theta)*rotx(alpha)
R = [Ct -St*Ca  St*Sa;
     St  Ct*Ca -Ct*Sa;
     0   Sa     Ca   ];  
%See equation 4.3 
d = [a*Ct; a*St; d];

%fill in the row of zeros below the rotation matrix
zt = zeros(1,size(R,1));

T = [R  d;
     zt 1];

[frygge]

Is there any progress regarding this PR? I am waiting for quaternions in the URDF for some time and wondering, whether it will be available in the current ROS noetic.

Best regards

[VictorLamoine]

I gave up because I don't like endless discussions. Feel free to take/improve my work into a new pull request!

[UniBwTAS]

We also would be happy when this pull request would be merged! The output of our extrinsic sensor calibration is in quaternion so there would be no need to convert them every time.

[UniBwTAS]

As a workaround we are able to use quaternions by using a xacro macro:

<xacro:macro name="quaternion_to_rpy" params="qx qy qz qw">
        <!-- See: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2 -->

        <xacro:property name="sinr_cosp" value="${2 * (qw * qx + qy * qz)}"/>
        <xacro:property name="cosr_cosp" value="${1 - 2 * (qx * qx + qy * qy)}"/>
        <xacro:property name="roll" value="${atan2(sinr_cosp, cosr_cosp)}" scope="parent"/>

        <xacro:property name="sinp" value="${2 * (qw * qy - qz * qx)}"/>
        <xacro:property name="pitch" value="${copysign(pi / 2, sinp) if fabs(sinp) >= 1 else asin(sinp)}"
                        scope="parent"/>

        <xacro:property name="siny_cosp" value="${2 * (qw * qz + qx * qy)}"/>
        <xacro:property name="cosy_cosp" value="${1 - 2 * (qy * qy + qz * qz)}"/>
        <xacro:property name="yaw" value="${atan2(siny_cosp, cosy_cosp)}" scope="parent"/>
    </xacro:macro>

It can be used like this:

<xacro:quaternion_to_rpy qx="0" qy="0" qz="0" qw="1"/>

<joint name="some_joint" type="fixed">
    <parent link="some_parent"/>
    <child link="some_child"/>
    <origin xyz="1 2 3" rpy="${roll} ${pitch} ${yaw}"/>
</joint>

[doisyg]

Reading this, it is a shame it did not go through.
Is there any related work conducted and planned since ?
If not, I might pick that one up in the next months.

[clalancette]

Is there any related work conducted and planned since ?

There's nothing planned that I know of. I'm not opposed to the idea at all, and now that we have URDF versions we can easily bump the version number. So do feel free to open a PR with this code in place plus a bump of the version. We'll also have to add support to https://github.com/ros/urdf_parser_py

[doisyg]

Created a new issue and PRs, see: #195