Resolve joints in nested models

[srmainwaring]

🦟 Bug fix

Fixes gz-sim: #1844

Summary

The details of the bug and how to reproduce it are documented in gz-sim: #1844.

In brief: joints in nested models were not being found correctly by the function EntityManagementFeatures::GetJoint which resulted in the Physics system plugin in gz-sim not reporting the joint position and not begin able to actuate the joint.

The change handles the three cases:

1. A joint between two links in a nested model.
2. A joint between a link in a top level model and link in a nested model.
3. A joint between two links in separate nested models.

There is also an issue where DART renames joints in nested models that have duplicate unscoped names. DART expects all names for a given skeleton (= gazebo model) to be unique. It is not practical to expect all Gazebo models to use different names, so we elected to register the joints for nested models using their top-level model scoped names. It may be worth doing this for links and collisions to avoid warnings such as:

Msg [NameManager::issueNewName] (Skeleton::BodyNode | model2) The name [base_link] is a duplicate, so it has been renamed to [base_link(1)]
Msg [NameManager::issueNewName] (Skeleton::N4dart8dynamics9ShapeNodeE | model2) The name [base_link:collision] is a duplicate, so it has been renamed to [base_link:collision(1)]
Msg [NameManager::issueNewName] (Skeleton::BodyNode | model2) The name [rotor_link] is a duplicate, so it has been renamed to [rotor_link(1)]
Msg [NameManager::issueNewName] (Skeleton::N4dart8dynamics9ShapeNodeE | model2) The name [rotor_link:collision] is a duplicate, so it has been renamed to [rotor_link:collision(1)]

but this would be for a separate PR.

An updated version of the test world is attached: test_nested_model.sdf.zip. Note: the tests were carried out using a modified version of the JointPositionController that handles nested models by searching for joints with scoped names. This modification will be submitted in gz-sim as a separate PR.

The joints are moved from the command line using:

# model1 works without this fix
#-----------------------------

# move the rotor in model1
gz topic -t "/model1/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# model2 illustrates case 1
#-----------------------------

# move the rotor in model2
gz topic -t "/model2/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# move the rotor in nested model21
gz topic -t "/model21/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# model3 illustrates case 2
#-----------------------------

# move the rotor in model3
gz topic -t "/model3/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# move the rotor in nested model31
gz topic -t "/model31/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# move the rotor in nested model32
gz topic -t "/model32/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# model4 illustrates case 3
#-----------------------------

# move the rotor in nested model41
gz topic -t "/model41/cmd_rotor" -m gz.msgs.Double -p "data: 1"

# move the rotor in nested model42
gz topic -t "/model42/cmd_rotor" -m gz.msgs.Double -p "data: 1"

Figure: test_nested_model.sdf showing all joints repositioned using the joint position controller plugin.

Testing (Updated)

There was one test failure in SDFFeatures_TEST (L601-L607). The test expected querying a parent model for a joint contained in a nested model to return a non-null pointer. However querying a parent for a grandchild joint should return null in the Gazebo interface even if DART moves the grand child joint to the parent.

  /// \todo(srmainwaring) this is failing. DART associates the nested joint
  /// with the skeleton of the top level model when the nested model is
  /// joined to the parent model, but Gazebo should not find grandchild
  /// joints when querying a parent model.
  auto nestedJoint = parentModel->GetJoint("nested_joint");
  // EXPECT_NE(nullptr, nestedJoint);
  EXPECT_EQ(nullptr, nestedJoint);

Checklist

• Signed all commits for DCO
• Added tests
• 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.

[azeey]

Thanks for the PR @srmainwaring. It looks like we were handling links in nested models properly for GetLink, but not joints in nested models for GetJoint. What do you think about storing a list of joints inside ModelInfo the same way we store links and nestedModels?

[srmainwaring]

What do you think about storing a list of joints inside ModelInfo the same way we store links and nestedModels?

Sure - that'd work and be consistent with the link handling. However with the links we still get a DART warning about renaming links when the nested model links are joined to the container model skeleton if there are any duplicated links (which is likely as most models will have a base_link). It's not clear when you see this message whether it is something to worry about, and whether it affects the way you should refer to links / joints in plugins.

IIRC the link code uses a world scoped full name for the links. If the same approach is used for nested joints, what is the convention for referring to these joints from plugins? It might be useful to have a document describing the various use cases for referring to joint and link names in plugins for flat and nested models and the Gazebo teams recommendation of best practice. I expect that needs to be something that works for other physics engines as well and is not just driven by DART's particular treatment.

[azeey]

It's not clear when you see this message whether it is something to worry about, and whether it affects the way you should refer to links / joints in plugins.

As you can see in LinkInfo, we store the original name of the link so that name changes by DART do not affect the way GetLink works.

gz-physics/dartsim/src/Base.hh

Lines 62 to 64 in 2a93515

	/// \brief It may be necessary for dartsim to rename a BodyNode (eg. when
	/// moving the BodyNode to a new skeleton), so we store the Gazebo-specified
	/// name of the Link here.

We could do the same for joints. Also, I think you're right in that we should use fully qualified names when adding entities to DART to avoid this name collision. However, we don't want this to leak into the user API of gz-physics. Ideally, we'd store the original local (unscoped) names of links and joints and allow GetLink and GetJoint to work with those names.

IIRC the link code uses a world scoped full name for the links.

It only uses the full names for other book keeping tasks. For GetLink, the user would still use the local/unscoped name

gz-physics/dartsim/src/EntityManagementFeatures.cc

Line 381 in 2a93515

if (_linkName == linkInfo->name)

We currently don't support accessing links of a nested model from the grand parent model. The issue addressed by keeping links in ModelInfo is the failure to find the body in the original skeleton because it has been moved due to a joint. I thought that's what we were trying to resolve for joints in this PR. I'd rather punt on accessing nested elements from grand parent models using scoped names. I think it's better to use entitiesFromScopedName in gz-sim for now.

[codecov]

Codecov Report

Merging #464 (8e338a9) into gz-physics6 (b85c93d) will increase coverage by 0.03%.
The diff coverage is 78.48%.

@@               Coverage Diff               @@
##           gz-physics6     #464      +/-   ##
===============================================
+ Coverage        75.52%   75.56%   +0.03%     
===============================================
  Files              142      142              
  Lines             7138     7190      +52     
===============================================
+ Hits              5391     5433      +42     
- Misses            1747     1757      +10     

Impacted Files	Coverage Δ
dartsim/src/EntityManagementFeatures.cc	83.06% <70.83%> (+0.28%)	⬆️
dartsim/src/SDFFeatures.cc	62.81% <76.92%> (+0.15%)	⬆️
dartsim/src/JointFeatures.cc	60.33% <80.00%> (+0.61%)	⬆️
dartsim/src/Base.hh	95.37% <85.18%> (-1.34%)	⬇️

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

[srmainwaring]

@azeey all makes sense. The PR for the JointPositionController gazebosim/gz-sim#1851 uses entitiesFromScopedName to resolve scoped names. There might need to be a little tweak given the changes suggested here, but I think for the most part it will work.

I'll start drafting your suggested changes in this PR following the approach for links.

[srmainwaring]

What do you think about storing a list of joints inside ModelInfo the same way we store links and nestedModels?

@azeey I've reworked the PR to use a similar approach to that used for links. The example provided above works.

I've promoted a method GetModelOfLinkImpl to dartsim::Base as it's required in JointFeatures which does not subclass from EntityManagementFeatures. There is some further consolidation that can be done in JointFeatures to reduce duplicated code, but would appreciate your feedback that this is on the right track before further refinements.

Update

We currently don't support accessing links of a nested model from the grand parent model

I've pushed a change to SDFFeatures_TEST::WorldWithNestedModel as I believe this is incorrectly expecting a parent model to find the joint of a nested model. DART does move the nested joint to the top-level model (skeleton) when the nested model is joined, but logically the joint belongs with the nested model (so this should be the behaviour when querying from Gazebo).

I've added some debug tools and notes as well - to be removed once the behaviour is agreed.

[azeey]

I've made some small comments, but the overall approach looks great!

[srmainwaring]

Hi @azeey - I think this PR now includes all the changes you requested and has been tidied up. Is there anything else you'd like completed to get this merged?

[azeey]

Just a couple of small comments. Otherwise, LGTM!