Support quaternions representation for poses

[aaronchongth]

🎉 New feature

Per proposal, http://sdformat.org/tutorials?tut=better_pose_proposal&cat=pose_semantics_docs&
Related to #252
Discussed briefly in #589

Summary

Support quaternions in the order of (x, y, z, w) for pose elements, using an new attribute //pose[@rotation_format], which is euler_rpy by default, but can be set to quat_xyzw.

• Poses that have a quaternion representation will be expected to have 7 values instead of the usual 6.
• Modified API for SetParentElement to return a bool in the event that the Reparse fails, for example having //pose[@degrees='true'][@rotation_format='quat_xyzw']
• When calling functions like ToString(), the exact value string that was provided in the .sdf file is expected. If left empty, the exact default value string that was defined in sdf/1.9/*.sdf will be expected. This is the reason some tests have been modified.
• Added tests and fixed broken tests.

These are valid poses,

<pose>1 2 3   0.4 0.5 0.6</pose>

<pose rotation_format="euler_rpy">1 2 3   0.4 0.5 0.6</pose>
      
<pose rotation_format="euler_rpy" degrees="true">1 2 3   90 180 270</pose>

<pose rotation_format="quat_xyzw">1 2 3   0.7071068 0 0 0.7071068</pose>

<pose rotation_format="quat_xyzw" degrees="false">1 2 3   0.7071068 0 0 0.7071068</pose>
      
<include>
  <uri>box</uri>
  <name>second_box</name>
  <pose rotation_format="quat_xyzw">0 10 0   0.7071068 0 0 0.7071068</pose>
</include>

These are not a valid pose,

<!--  An empty value uses the default 6-tuple, which will not make up a quaternion which expects a 7-tuple -->
<pose rotation_format="quat_xyzw"></pose>

<!-- Defining a quaternion and using degrees will cause a failure to parse too -->
<pose rotation_format="quat_xyzw" degrees="true">1 2 3   0.7071068 0 0 0.7071068</pose>

Test

source install/setup.bash
colcon test --packages-select sdformat12

Checking representations,

source install/setup.bash

ign sdf -p WORKSPACE/src/sdformat/test/sdf/pose_1_9.sdf

# includes
export SDF_PATH=WORKSPACE/src/sdformat/test/integration/model
ign sdf -p WORKSPACE/src/sdformat/test/sdf/include_pose_1_9.sdf

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[codecov-commenter]

Codecov Report

Merging #690 (70accc4) into main (3b4d136) will increase coverage by 0.08%.
The diff coverage is 94.62%.

❗ Current head 70accc4 differs from pull request most recent head 697d143. Consider uploading reports for the commit 697d143 to get more accurate results

@@            Coverage Diff             @@
##             main     #690      +/-   ##
==========================================
+ Coverage   88.06%   88.14%   +0.08%     
==========================================
  Files          75       75              
  Lines       11333    11363      +30     
==========================================
+ Hits         9980    10016      +36     
+ Misses       1353     1347       -6     

Impacted Files	Coverage Δ
include/sdf/Param.hh	77.14% <ø> (ø)
src/Param.cc	91.13% <93.15%> (+1.95%)	⬆️
src/Element.cc	96.82% <100.00%> (ø)
src/parser.cc	86.03% <100.00%> (+0.07%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 3b4d136...697d143. Read the comment docs.

[azeey]

I'm getting a few test regressions in ign-physics and ign-gazebo with this PR. I can't seem to figure what's causing them though.

[azeey]

I'm getting a few test regressions in ign-physics and ign-gazebo with this PR. I can't seem to figure what's causing them though.

I believe some of the regressions caused by the use of ValueFromString for computing min and max values in:
https://github.com/ignitionrobotics/sdformat/blob/ea6729fbaa4915340343496f6928cb448b4620b0/src/Param.cc#L90-L100

Instead of the default value, strValue ends up having the max value since ValueFromString is called on max values last. Of course, this only affects values that have min and max constraints.

Using ValueFromStringImpl should fix the issue. You'll have to pass this->dataPtr->minValue.emplace() as the third argument because minValue would not contain any value at that call site. Same for maxValue.

The regressions in ign-gazebo on tests UNIT_SdfGenerator_TEST and INTEGRATION_save_world seem to be caused by a different issue. I see a lot of assertions with the message: "Cannot set parent Element of cloned attribute Param to cloned Element."

[aaronchongth]

The regressions in ign-gazebo on tests UNIT_SdfGenerator_TEST and INTEGRATION_save_world seem to be caused by a different issue. I see a lot of assertions with the message: "Cannot set parent Element of cloned attribute Param to cloned Element."

@azeey, I've managed to fix the regression tests by adding this check for the attribute, as there are some pose elements that don't support degrees and rotation_format, like actor and urdf

[azeey]

Other than the issue below, this looks good. I propose we create an issue for it and merge this PR. @jennuine or @scpeters , do you mind taking a look at the changes I made in fcac14d

[azeey]

When parsing <pose />, the _input will not actually be empty, it will be the default value of //pose because in Param(), calls ValueFromString with the default string https://github.com/ignitionrobotics/sdformat/blob/fcac14d3c9359910c12fcf029eb70ca468e3220e/src/Param.cc#L76 and ValueFromString sets strValue to whatever it's input argument is:
https://github.com/ignitionrobotics/sdformat/blob/fcac14d3c9359910c12fcf029eb70ca468e3220e/src/Param.cc#L586-L593

I think this is why <pose rotation_format='quat_xyzw/>` doesn't work currently.

[azeey]

Ticketed an issue for this #707.

[jennuine]

@jennuine or @scpeters , do you mind taking a look at the changes I made in fcac14d

fcac14d LGTM, left one minor nit

[scpeters]

@jennuine or @scpeters , do you mind taking a look at the changes I made in fcac14d

fcac14d LGTM, left one minor nit

same, it LGTM with minor nits

[azeey]

@jennuine or @scpeters , do you mind taking a look at the changes I made in fcac14d

fcac14d LGTM, left one minor nit

Done in 1f542c4

nit: ASSERT_* statements will abort the test if they fail. I find them useful when checking that pointers aren't nullptr before dereferencing them in subsequent parts of the test, but I generally prefer EXPECT_* if a failed expectation will not cause the test to crash

so consider if ASSERT_TRUE is needed for these new expectations

I agree, I changed them to EXPECT_EQ in ae929b5

[scpeters]

just two small nits

[scpeters]

I propose we create an issue for it and merge this PR.

I'll approve once we have this issue created

[scpeters]

I propose we create an issue for it and merge this PR.

I'll approve once we have this issue created

thanks for opening #707.

[scpeters]

nice work!

whoever does the squash and merge, please remember the merging strategy in our process documentation involves editing the commit message in the GitHub UI to "captures the core ideas of the pull request and contains all authors' signatures". By default these messages contain lots of unnecessary details about the commit history of the pull request, rather than a succinct summary.