Fix xyz and rpy offsets in fixed joint reduction #500

iche033 · 2021-02-23T05:17:30Z

Continuation of the fix from pull request #497. This is another report and contribution from external user.

The xyzOffset and rpyOffset computation seems to be incorrect for a model plugin during fixed joint reduction.

This diagram describes the urdf configuration that is being tested:

The model has three links connected together by two fixed joints with [0 1 0] pos and [0 0 pi/4] rotation joint origin as shown in the diagram (see test/integration/fixed_joint_reduction_plugin_frame_extension.urdf). The model plugin has these relevant params:

    <plugin name='test_plugin' filename='libtest_plugin.so'>
      ...
      <bodyName>link2</bodyName>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 0 0</rpyOffset>
    </plugin>

After fixed joint reduction, link1 and link2 are merged into base_link and hence the xyzOffset and rpyOffset values are also modified to be in the base_link frame

Before the changes: the urdf parser produces these offsets:

bodyName: "base_link"
xyzOffset: -0.707108, -1.70711, 0
rpyOffset: 0, 0, -1.5708

Looking at the offsets and also the diagram above, these values do not look correct.

After the changes: the urdf parser produces these offsets:

bodyName: "base_link"
xyzOffset: -0.707108, 1.70711, 0
rpyOffset: 0, 0, 1.5708

These values match what's shown in the diagram.

Note the code has been around for many years so I appreciate a closer look at this. Looking at gazebo_ros_pkgs, the one plugin that could be affected is the gazebo_ros_p3d plugin, which is a model plugin and makes use of these offsets. The issue only happens if the user has this plugin and references a frame that's being reduced due to fixed joints. Here's a related issue in gazebo_ros_pkgs. The comments suggest that people have worked around it by disabling fixed joint lumping or using continuous joints with zero limits.

Signed-off-by: Ian Chen ichen@osrfoundation.org

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

azeey · 2021-02-24T19:01:19Z

The fact that it generates

xyzOffset: -0.707108, -1.70711, 0
rpyOffset: 0, 0, -1.5708

before the updates in this PR confuse me. If it was doing the inverse (i.e, base_link relative to link2), wouldn't it be
xyzOffset: -1.70711, -0.707108, 0
rpyOffset: 0, 0, -1.5708

Aside from that, the changes makes sense to me, but like you said, this is problematic for gazebo_ros_p3d. Particularly, gazebo_ros_p3d.cpp#L295 has

        pose.Rot() = this->offset_.Rot()*pose.Rot();

If we merge this PR, we need to update that line to

        pose.Rot() = pose.Rot() * this->offset_.Rot();

iche033 · 2021-02-24T20:21:13Z

The inverseTransformToParentFrame function could be doing the math wrong?

If I set the bodyName to link1 (instead of link2), I get an xyzOffset of 0 -1 0 0 0 -0.7854 but I would expect it to produce something like -0.707108 -0.707108 0 (base_link relative to link1)

azeey · 2021-02-24T22:49:48Z

I worked through the math in inverseTransformToParentFrame and it doesn't quite come out to the inverse of TransformToParentFrame.

The code currently does:
R_BL^-1 p_LQ - p_BL

but for it to be the inverse of TransformToParentFrame, I think it should be:
R_BL^-1 p_LQ - R_BL^-1 p_BL

where:
R_BL : _parentToLinkTransform.rotation
p_LQ : _transformInLinkFrame.Pos()
p_BL: _parentToLinkTransform.position

Also, I noticed that with the changes in this PR, inverseTransformToParentFrame is not used anymore.

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 · 2021-02-26T01:30:13Z

thanks for checking the math. I've removed the inverseTransformToParentFrame function since that it's incorrect and no longer used. de3e268.

iche033 · 2021-02-26T01:59:03Z

here is the gazebo_ros_pkgs PR for the updating the rotation offset calculation: ros-simulation/gazebo_ros_pkgs#1241

chapulina · 2021-07-10T01:56:16Z

What's the status of this PR? It's been approved for a while.

scpeters · 2021-07-14T04:34:13Z

fix for Ubuntu CI in #626

codecov-commenter · 2021-07-14T05:56:33Z

Codecov Report

Merging #500 (26deff0) into sdf6 (b06cc18) will increase coverage by 0.69%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##             sdf6     #500      +/-   ##
==========================================
+ Coverage   65.70%   66.40%   +0.69%     
==========================================
  Files          88       88              
  Lines       10163    10160       -3     
==========================================
+ Hits         6678     6747      +69     
+ Misses       3485     3413      -72

Impacted Files	Coverage Δ
src/parser_urdf.cc	`82.87% <100.00%> (+3.93%)`	⬆️
test/integration/fixed_joint_reduction.cc	`91.91% <100.00%> (+0.29%)`	⬆️
src/urdf/urdf_model/pose.h	`84.21% <0.00%> (+8.91%)`	⬆️

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 b06cc18...26deff0. Read the comment docs.

scpeters · 2021-07-14T20:18:36Z

I wonder if we should try to inject a <ignition:corrected_offsets>true</ignition:corrected_offsets> tag into the plugin block to simplify the logic in gazebo_plugins required to handle this? I think this will be easier than tracking which version of libsdformat has the behavior change

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 · 2021-07-16T06:15:30Z

inject a ignition:corrected_offsetstrue</ignition:corrected_offsets> tag into the plugin block

alright I implemented this suggestion in 536b847. One question, do we need the ignition namespace?

scpeters · 2021-07-16T07:11:41Z

inject a ignition:corrected_offsetstrue</ignition:corrected_offsets> tag into the plugin block

alright I implemented this suggestion in 536b847. One question, do we need the ignition namespace?

well, since it's in a plugin none of the tags are part of the spec, so a namespace wouldn't be required for that reason, but since we are injecting the element, I feel better about using a namespace so we don't conflict with any preexisting user tags. I think it's unlikely, so we could probably be fine without the namespace, but I have a slight preference for it. If others feel more strongly about not using ignition:, I'm happy to go along with removing the namespace

iche033 · 2021-07-16T16:48:48Z

ok sounds good! I'll keep the ignition namespace

scpeters · 2021-07-23T20:52:53Z

I just noticed that it seems to be inserting two <ignition::corrected_offsets>1</ignition::corrected_offsets> tags

scpeters · 2021-07-23T20:55:06Z

src/parser_urdf.cc

@@ -3473,14 +3444,21 @@ void ReduceSDFExtensionPluginFrameReplace(
          // remove xyzOffset and rpyOffset
          (*_blobIt)->RemoveChild(rpyKey);
        }
+        TiXmlNode* correctedOffsetKey =
+            (*_blobIt)->FirstChild("ignition::corrected_offsets");
+        if (!correctedOffsetKey)


I think the logic is backwards here. Should the ! be removed?

removed in 2eba874

Signed-off-by: Steven Peters <scpeters@openrobotics.org>

scpeters · 2021-07-23T20:58:58Z

I just noticed that it seems to be inserting two <ignition::corrected_offsets>1</ignition::corrected_offsets> tags

should be fixed by 2eba874

iche033 · 2021-07-23T22:50:07Z

should be fixed by 2eba874

thanks!

There is special handling in the parser_urdf code to update plugin fields when links are merged together during fixed joint reduction. A test for this was added to sdf6 in gazebosim#500. A portion of this test is applied directly to the sdf10 branch to illustrate that it is broken. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

There is special handling in the parser_urdf code to update plugin fields when links are merged together during fixed joint reduction. A test for this was added to sdf6 in #500. A portion of this test is applied directly to the sdf10 branch to illustrate a problem with ReduceSDFExtensionPluginFrameReplace in parser_urdf.cc. The original migration to use tinyxml2 in #264 changed the data structure used in SDFExtension to store XMLDocuments instead of XMLPointers, which requires an extra call to FirstChildElement, but the ReduceSDFExtension*FrameReplace functions did not receive this update. The fix here refactors the function arguments to pass the first child element directly, which simplifies the helper function implementation. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

osrf-triage · 2022-01-10T21:45:57Z

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-01-10/1228/1

osrf-triage · 2022-03-25T22:29:48Z

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-03-25-fortress-edifice-citadel/1343/1

iche033 added 2 commits February 17, 2021 19:02

parse rpyOffset as radians

9ca30cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

update tf for xyz and rpy offsets

72381f1

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 requested review from azeey and scpeters February 23, 2021 05:17

remove inverse transform function in urdf parser

de3e268

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

azeey approved these changes Mar 1, 2021

View reviewed changes

iche033 mentioned this pull request Mar 3, 2021

Reverse order of rotation offset (rpyOffset) calculation in p3d and imu plugins ros-simulation/gazebo_ros_pkgs#1241

Open

Base automatically changed from iche033/urdr_parser_rpy to sdf6 March 16, 2021 22:27

Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame

1e81e4a

chapulina added this to Inbox in Core development via automation Jun 28, 2021

chapulina moved this from Inbox to In review in Core development Jun 28, 2021

scpeters mentioned this pull request Jul 14, 2021

Use Ubuntu bionic in CI #626

Merged

7 tasks

Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame

9bffb10

scpeters approved these changes Jul 14, 2021

View reviewed changes

inject corrected_offets tag

536b847

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

scpeters reviewed Jul 23, 2021

View reviewed changes

Fix tag removal logic

2eba874

Signed-off-by: Steven Peters <scpeters@openrobotics.org>

Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame

26deff0

iche033 merged commit c02aca7 into sdf6 Aug 30, 2021

iche033 deleted the iche033/urdf_parser_plugin_frame branch August 30, 2021 20:59

Core development automation moved this from In review to Done Aug 30, 2021

scpeters mentioned this pull request Nov 9, 2021

9 to 10 20211108 #741

Closed

This was referenced Nov 11, 2021

Fix URDF fixed joint reduction of plugins #745

Merged

parser_urdf: ReduceSDFExtension*FrameReplace functions are broken in libsdformat10+ #746

Open

azeey mentioned this pull request Mar 1, 2022

Add xyz and rpy offset to published odometry pose gazebosim/gz-sim#1341

Merged

8 tasks

j-rivero removed this from Done in Core development May 6, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix xyz and rpy offsets in fixed joint reduction #500

Fix xyz and rpy offsets in fixed joint reduction #500

iche033 commented Feb 23, 2021 •

edited

Loading

azeey commented Feb 24, 2021

iche033 commented Feb 24, 2021 •

edited

Loading

azeey commented Feb 24, 2021

iche033 commented Feb 26, 2021

iche033 commented Feb 26, 2021

chapulina commented Jul 10, 2021

scpeters commented Jul 14, 2021

codecov-commenter commented Jul 14, 2021 •

edited

Loading

scpeters commented Jul 14, 2021 •

edited

Loading

iche033 commented Jul 16, 2021

scpeters commented Jul 16, 2021

iche033 commented Jul 16, 2021

scpeters commented Jul 23, 2021

scpeters Jul 23, 2021

scpeters Jul 23, 2021

scpeters commented Jul 23, 2021

iche033 commented Jul 23, 2021

osrf-triage commented Jan 10, 2022

osrf-triage commented Mar 25, 2022

Fix xyz and rpy offsets in fixed joint reduction #500

Fix xyz and rpy offsets in fixed joint reduction #500

Conversation

iche033 commented Feb 23, 2021 • edited Loading

azeey commented Feb 24, 2021

iche033 commented Feb 24, 2021 • edited Loading

azeey commented Feb 24, 2021

iche033 commented Feb 26, 2021

iche033 commented Feb 26, 2021

chapulina commented Jul 10, 2021

scpeters commented Jul 14, 2021

codecov-commenter commented Jul 14, 2021 • edited Loading

Codecov Report

scpeters commented Jul 14, 2021 • edited Loading

iche033 commented Jul 16, 2021

scpeters commented Jul 16, 2021

iche033 commented Jul 16, 2021

scpeters commented Jul 23, 2021

scpeters Jul 23, 2021

Choose a reason for hiding this comment

scpeters Jul 23, 2021

Choose a reason for hiding this comment

scpeters commented Jul 23, 2021

iche033 commented Jul 23, 2021

osrf-triage commented Jan 10, 2022

osrf-triage commented Mar 25, 2022

iche033 commented Feb 23, 2021 •

edited

Loading

iche033 commented Feb 24, 2021 •

edited

Loading

codecov-commenter commented Jul 14, 2021 •

edited

Loading

scpeters commented Jul 14, 2021 •

edited

Loading