Support quaternions representation for poses by aaronchongth · Pull Request #690 · gazebosim/sdformat

aaronchongth · 2021-09-07T08:09:11Z

🎉 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 · 2021-09-07T08:26:02Z

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.

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…y pose string Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…tation formats Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

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.

…n description Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

azeey · 2021-09-17T03:39:46Z

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."

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…ving hard coded default value from ValueFromStringImpl Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…::string, used ValueFromStringImpl for min and max value Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth · 2021-09-20T08:17:22Z

Thanks for catching the mistakes regarding min and max values, I've corrected them in 0fdcd97. I'll continue looking into the regressions

…at don't use 1.9/pose.sdf Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth · 2021-09-20T10:25:53Z

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

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…uler_rpy Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…he value is parsed successfully Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

…ns from previous test Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

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 · 2021-09-20T19:53:00Z

+    return false;
+  }
+
+  std::string input = _input.empty() ? defaultValueStr : _input;


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.

Ticketed an issue for this #707.

jennuine · 2021-09-20T21:46:13Z

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

fcac14d LGTM, left one minor nit

scpeters · 2021-09-20T23:05:18Z

@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

…on-support

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

azeey · 2021-09-21T00:08:16Z

@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

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

scpeters · 2021-09-21T04:16:06Z

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

I'll approve once we have this issue created

scpeters · 2021-09-21T07:59:03Z

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.

github-actions bot added the 🏯 fortress Ignition Fortress label Sep 7, 2021

chapulina added the beta Targeting beta release of upcoming collection label Sep 8, 2021

azeey mentioned this pull request Sep 9, 2021

Update document to better reflect proposed and accepted implementations gazebosim/sdf_tutorials#56

Merged

aaronchongth force-pushed the aaron/pose-quaternion-support branch 2 times, most recently from ac7ca0a to 73a1a49 Compare September 14, 2021 06:48

aaronchongth added 8 commits September 17, 2021 01:02

Added attribute @rotation_format to //pose, supporting rpy and q_wxyz

83449e5

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Param parsing rotation_format

dc2da47

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Added rotation_format to testing sdf and added integration test

22a6614

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

SetParentElement to return bool, added tests to handle quat_wxyz

2059da8

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Fixed tests comparing string outputs, added else case to parsing empt…

a23dad4

…y pose string Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Added asserts to setting parent elements to params

9ace44d

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Added further tests for SetParentElement API and switching between ro…

6f94f1d

…tation formats Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Switched to quat_xyzw

3c151fa

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth force-pushed the aaron/pose-quaternion-support branch from 517dcea to 3c151fa Compare September 16, 2021 17:03

aaronchongth marked this pull request as ready for review September 16, 2021 17:03

aaronchongth requested a review from azeey September 16, 2021 17:03

aaronchongth added 2 commits September 17, 2021 01:12

Fixed missing degrees attribute test

df7ee2c

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Added other representations in include tests

ea6729f

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

azeey requested changes Sep 16, 2021

View reviewed changes

Reverted removed curly braces, added quotations to rotation formats i…

d59b65f

…n description Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth added 4 commits September 19, 2021 12:27

Reverted modifications of past tests that may break downstream tests

ae796a6

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Changing name of test to be more explicit

7e017fc

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Correcting pose parsing logic, checking attributes at the start, remo…

d78976a

…ving hard coded default value from ValueFromStringImpl Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Added defaultValueStr field for use in Reset, changed strValue to std…

0fdcd97

…::string, used ValueFromStringImpl for min and max value Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth force-pushed the aaron/pose-quaternion-support branch from 0b81a05 to 0fdcd97 Compare September 20, 2021 08:15

Added checks for attributes in readXml, as there are pose elements th…

2250af4

…at don't use 1.9/pose.sdf Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

aaronchongth added 4 commits September 20, 2021 23:12

Fixing tests to expect default values as defined in sdf/1.9/*.sdf

23fce80

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Changing default rotation_format as empty string to test setting to e…

c600f48

…uler_rpy Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Reparsing if no value was stated in xml, only modifying strValue if t…

40d670a

…he value is parsed successfully Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

Adding tests for bad pose representations, removing bad representatio…

7b09f70

…ns from previous test Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

scpeters mentioned this pull request Sep 20, 2021

Merge 11 -> 12 #706

Merged

aaronchongth requested a review from azeey September 20, 2021 19:30

Add @rotation_format and @Degrees to //inertial/pose

fcac14d

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

azeey approved these changes Sep 20, 2021

View reviewed changes

Addisu Z. Taddese added 3 commits September 20, 2021 18:27

Merge remote-tracking branch 'upstream/main' into aaron/pose-quaterni…

69a0f57

…on-support

Address reviewer feedback

1f542c4

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Change ASSERT_EQ to EXPECT_EQ

ae929b5

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

jennuine approved these changes Sep 21, 2021

View reviewed changes

scpeters reviewed Sep 21, 2021

View reviewed changes

Comment thread test/sdf/pose_1_9.sdf Outdated

Comment thread test/integration/pose_1_9_sdf.cc Outdated

Fix test models

697d143

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

azeey mentioned this pull request Sep 21, 2021

Empty pose not interpreted as identity when rotation_format=quat_xyzw is set #707

Closed

scpeters approved these changes Sep 21, 2021

View reviewed changes

aaronchongth merged commit ef2bc64 into main Sep 21, 2021

aaronchongth deleted the aaron/pose-quaternion-support branch September 21, 2021 15:23

Conversation

aaronchongth commented Sep 7, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🎉 New feature

Summary

Test

Checklist

Uh oh!

codecov-commenter commented Sep 7, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

azeey left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

azeey commented Sep 17, 2021

Uh oh!

aaronchongth commented Sep 20, 2021

Uh oh!

aaronchongth commented Sep 20, 2021

Uh oh!

azeey left a comment

Choose a reason for hiding this comment

Uh oh!

azeey Sep 20, 2021

Choose a reason for hiding this comment

Uh oh!

azeey Sep 21, 2021

Choose a reason for hiding this comment

Uh oh!

jennuine commented Sep 20, 2021

Uh oh!

scpeters commented Sep 20, 2021

Uh oh!

azeey commented Sep 21, 2021

Uh oh!

scpeters left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

scpeters commented Sep 21, 2021

Uh oh!

scpeters commented Sep 21, 2021

Uh oh!

scpeters left a comment

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

8 participants

aaronchongth commented Sep 7, 2021 •

edited

Loading

codecov-commenter commented Sep 7, 2021 •

edited

Loading