Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix xyz and rpy offsets in fixed joint reduction #500

Merged
merged 8 commits into from
Aug 30, 2021

Conversation

iche033
Copy link
Contributor

@iche033 iche033 commented Feb 23, 2021

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:

fixed_joint_offset_tf

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 [email protected]

@azeey
Copy link
Collaborator

azeey commented Feb 24, 2021

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
Copy link
Contributor Author

iche033 commented Feb 24, 2021

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
Copy link
Collaborator

azeey commented Feb 24, 2021

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.

@iche033
Copy link
Contributor Author

iche033 commented Feb 26, 2021

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

@iche033
Copy link
Contributor Author

iche033 commented Feb 26, 2021

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

@chapulina
Copy link
Contributor

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

@scpeters scpeters mentioned this pull request Jul 14, 2021
7 tasks
@scpeters
Copy link
Member

fix for Ubuntu CI in #626

@codecov-commenter
Copy link

codecov-commenter commented Jul 14, 2021

Codecov Report

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

Impacted file tree graph

@@            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
Copy link
Member

scpeters commented Jul 14, 2021

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

@iche033
Copy link
Contributor Author

iche033 commented Jul 16, 2021

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
Copy link
Member

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
Copy link
Contributor Author

iche033 commented Jul 16, 2021

ok sounds good! I'll keep the ignition namespace

@scpeters
Copy link
Member

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

removed in 2eba874

Signed-off-by: Steven Peters <[email protected]>
@scpeters
Copy link
Member

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

should be fixed by 2eba874

@iche033
Copy link
Contributor Author

iche033 commented Jul 23, 2021

should be fixed by 2eba874

thanks!

@iche033 iche033 merged commit c02aca7 into sdf6 Aug 30, 2021
@iche033 iche033 deleted the iche033/urdf_parser_plugin_frame branch August 30, 2021 20:59
@scpeters scpeters mentioned this pull request Nov 9, 2021
scpeters added a commit to scpeters/sdformat that referenced this pull request Nov 10, 2021
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 <[email protected]>
scpeters added a commit that referenced this pull request Nov 12, 2021
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 <[email protected]>
@osrf-triage
Copy link

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
Copy link

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants