parser_urdf: add //frame for reduced links/joints #1148

scpeters · 2022-09-16T16:29:43Z

🎉 New feature

Retain frame information from URDF fixed joints that are not preserved, related to #1110

Summary

Currently the location of some links and joints is lost when fixed joints are reduced and links lumped together. For example, the first snippet below is a URDF with two links outlined in red and a fixed joint outlined in blue. The child link has a visual outlined in green. The second snippet shows what results when converting to SDFormat, with the fixed joint and child link removed, and the visual moved to the parent link.

This process of "fixed joint reduction" reduces the complexity of a simulation, particularly for solvers using maximal coordinates, but it does lose some frame information. This pull request adds //model/frame tags with the same name and pose of the reduced links and joints. For example, the following would be added for the examples shown above.

    <frame name='fixed_joint' attached_to='base'>
      <pose>0 0 0 0 0 0</pose>
    </frame>
    <frame name='child_link' attached_to='fixed_joint'>
      <pose>0 0 0 0 0 0</pose>
    </frame>

Test it

Compile and run INTEGRATION_urdf_gazebo_extensions

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if the library has them)
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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

codecov · 2022-09-16T16:35:22Z

Codecov Report

Merging #1148 (2e9f08e) into sdf9 (b7f0cba) will increase coverage by 0.11%.
The diff coverage is 91.30%.

@@            Coverage Diff             @@
##             sdf9    #1148      +/-   ##
==========================================
+ Coverage   87.82%   87.94%   +0.11%     
==========================================
  Files          64       64              
  Lines       10095    10118      +23     
==========================================
+ Hits         8866     8898      +32     
+ Misses       1229     1220       -9

Impacted Files	Coverage Δ
src/parser_urdf.cc	`85.15% <91.30%> (+0.79%)`	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

Currently the location of some links and joints is lost when fixed joints are reduced and links lumped together. This adds //model/frame tags with the same name and pose of the reduced links and joints. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

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

scpeters · 2022-09-21T22:33:29Z

this is ready for review now that #1126 has been merged

adityapande-1995

Looks good to me !

adityapande-1995 · 2022-09-23T23:55:19Z

test/integration/urdf_gazebo_extensions.cc

+  ASSERT_NE(nullptr, link);
+  auto massMatrix = link->Inertial().MassMatrix();
+  EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass());
+  EXPECT_EQ(2.0 * ignition::math::Matrix3d::Identity, massMatrix.Moi());


Nit : Can this be changed to gz namespace ? I guess not, since this is targeting an older sdf branch.

it would be possible, but we would also have to bump the minimum required version of ignition-math6 to 6.13.0. These changes are already being handled in #1118, and it's slightly simpler for VIPER if we can merge this PR without the gz:: namespaces, so I'd prefer to defer that change for now

azeey

Just a few minor comments. Otherwise, this is a nice feature and LGTM!

azeey · 2022-09-28T19:49:00Z

src/parser_urdf.cc

+    // Ensure model extension vector is allocated
+    if (g_extensions.find("") == g_extensions.end())
+    {
+      std::vector<SDFExtensionPtr> ge;
+      g_extensions.insert(std::make_pair("", ge));
+    }
+
+    // Add //frame tags to model extension vector
+    g_extensions.at("").push_back(sdfExt);


nit: This would do the same thing, I believe.

Suggested change

// Ensure model extension vector is allocated

if (g_extensions.find("") == g_extensions.end())

{

std::vector<SDFExtensionPtr> ge;

g_extensions.insert(std::make_pair("", ge));

}

// Add //frame tags to model extension vector

g_extensions.at("").push_back(sdfExt);

g_extensions[""].push_back(sdfExt);

good call: simplified in 2e9f08e

azeey · 2022-09-28T19:54:47Z

src/parser_urdf.cc

+    auto stringToExtension = [&sdfExt](const std::string &_frame)
+    {
+      TiXmlDocument xmlNewDoc;
+      xmlNewDoc.Parse(_frame.c_str());


Is it possible for this to fail? i.e, do we need to check if xmlNewDoc.FirstChildElement() is nullptr?

there's lots of places where we aren't doing proper error checking, but I've added some for this new code in 2e9f08e

azeey · 2022-09-28T20:10:44Z

test/integration/urdf_gazebo_extensions.cc


-  sdf::ElementPtr model = robot->Root()->GetElement("model");
+  auto modelDom = root.ModelByIndex(0);


Add an assertion on modelDom.

done in 2e9f08e

azeey · 2022-09-28T20:15:47Z

test/integration/fixed_joint_example.urdf

+  </joint>
+  <link name="rotary_link">
+    <inertial>
+      <mass value="12"/>


Can you add a comment that this <inertial> is set with the expected value of the <inerital> of base and intermediate_link combined?

I added some clarifying comments about the model in 2e9f08e, including what you have suggested here

azeey · 2022-09-28T20:16:23Z

test/integration/urdf_gazebo_extensions.cc

+  EXPECT_EQ(1u, model->LinkCount());
+  EXPECT_TRUE(model->LinkNameExists("base"));
+
+  // Expect MassMatrix3 values to match for links


nit: remove?

done in 2e9f08e

* Check for tinyxml2 parsing errors * Simplify map logic * Add comments to example URDF * Assert point is valid in test * Remove outdated comment Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Follow-up for #1148 with a cleaner implementation for the sdf13 branch that constructs sdf::Frame objects and serializes them to a string rather than using a stringstream to construct XML. Signed-off-by: Steve Peters <scpeters@openrobotics.org>

scpeters requested a review from azeey as a code owner September 16, 2022 16:29

traversaro mentioned this pull request Sep 16, 2022

URDF to SDF conversion ignores links without inertia #199

Closed

scpeters mentioned this pull request Sep 16, 2022

parser_urdf: add //frame for reduced links/joints (garden) #1149

Closed

9 tasks

Base automatically changed from scpeters/fix_urdf_gz_ext_test to sdf9 September 21, 2022 21:21

scpeters added 3 commits September 21, 2022 15:32

Add another test with simpler example URDF

fe3b393

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

Add test with even simpler example

8b3c2ad

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

scpeters force-pushed the scpeters/urdf_fixed_joint_frames_9 branch from 7760525 to 8b3c2ad Compare September 21, 2022 22:32

scpeters requested review from adityapande-1995 and jennuine September 23, 2022 00:38

adityapande-1995 approved these changes Sep 23, 2022

View reviewed changes

scpeters added 🏰 citadel Ignition Citadel Gazebo 1️1️ Dependency of Gazebo classic version 11 labels Sep 28, 2022

azeey approved these changes Sep 28, 2022

View reviewed changes

Respond to review comments

2e9f08e

* Check for tinyxml2 parsing errors * Simplify map logic * Add comments to example URDF * Assert point is valid in test * Remove outdated comment Signed-off-by: Steve Peters <scpeters@openrobotics.org>

scpeters merged commit 26569a5 into sdf9 Sep 29, 2022

scpeters deleted the scpeters/urdf_fixed_joint_frames_9 branch September 29, 2022 01:56

scpeters mentioned this pull request Sep 30, 2022

Merge sdf12 ➡️ sdf13 #1182

Merged

scpeters mentioned this pull request Oct 18, 2022

Feature: merge a fixed joint's child link to the parent #1110

Open

scpeters mentioned this pull request Nov 3, 2022

Fix static URDF models with fixed joints #1193

Merged

8 tasks

traversaro mentioned this pull request Mar 7, 2024

Add sensors to mujoco ergoCub ami-iit/mujoco-urdf-loader#6

Open

diegoferigo mentioned this pull request May 16, 2024

Enhance processing and URDF export of //frame elements ami-iit/rod#34

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

parser_urdf: add //frame for reduced links/joints #1148

parser_urdf: add //frame for reduced links/joints #1148

scpeters commented Sep 16, 2022 •

edited

Loading

codecov bot commented Sep 16, 2022 •

edited

Loading

scpeters commented Sep 21, 2022

adityapande-1995 left a comment

adityapande-1995 Sep 23, 2022

scpeters Sep 26, 2022

azeey left a comment

azeey Sep 28, 2022

scpeters Sep 28, 2022

azeey Sep 28, 2022

scpeters Sep 28, 2022 •

edited

Loading

azeey Sep 28, 2022

scpeters Sep 28, 2022

azeey Sep 28, 2022

scpeters Sep 28, 2022

azeey Sep 28, 2022

scpeters Sep 28, 2022


		sdf::ElementPtr model = robot->Root()->GetElement("model");
		auto modelDom = root.ModelByIndex(0);

parser_urdf: add //frame for reduced links/joints #1148

parser_urdf: add //frame for reduced links/joints #1148

Conversation

scpeters commented Sep 16, 2022 • edited Loading

🎉 New feature

Summary

Test it

Checklist

codecov bot commented Sep 16, 2022 • edited Loading

Codecov Report

scpeters commented Sep 21, 2022

adityapande-1995 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

azeey left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

scpeters Sep 28, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

scpeters commented Sep 16, 2022 •

edited

Loading

codecov bot commented Sep 16, 2022 •

edited

Loading

scpeters Sep 28, 2022 •

edited

Loading