Map canonical links to their models

[adlarkin]

Signed-off-by: Ashton Larkin ashton@openrobotics.org

🎉 New feature

Related to #706

Follow-up to #685 and #678

Summary

When calling UpdateSim in the physics system, we have to update a model's pose if the model's canonical link changed. We are currently handling this by iterating through all models in the ECM because:

1. The ECM implicitly gives us a topological ordering of models (Support individual canonical links for nested models #685 (comment)), which allows us to handle nested model updates correctly since nested model pose calculations depend on the parent model pose.
2. For each changed canonical link, we have no way of knowing which models view this link as its canonical link, unless we recurse upward via parent models, checking model canonical links along the way. This is inefficient to do for every canonical link in each simulation update, and is also difficult code to maintain/understand. So, we instead get each model and check if its canonical link has changed.

This PR should resolve both of these issues, and also removes the need for looping through the ECM to check for model updates, which should result in a performance improvement for worlds with a lot of non-static models that do not change often (see #711 to learn more about why this change results in a performance improvement).

Test it

The physics system integration test has been modified in this PR (click on the files changed tab). However, to get a better grasp of the performance improvement that this PR creates, a testing methodology similar to #678 was used:

• 3000 non-static (or static) simple shapes sitting on a static ground plane
• running headless (-s), with the default server config

Test results are shown below. "Before this PR" uses commit 4e40b36 of ign-gazebo. Notice how the yellow Models block is now essentially gone for most of the tests cases now, except for the non-static case with DART (see the Takeaways section below for more information about this).

It's important to note that running the profiler decreases performance, so the RTF numbers reported here are with the profiler turned off.

TPE

Static

Before this PR:

• RTF: 50%
• UpdateSim time: 0.67ms

After this PR:

• RTF: 85% 😀
• UpdateSim time: 0.004ms

Non-Static

Before this PR:

• RTF: 14.75%
• UpdateSim time: 0.8ms

After this PR:

• RTF: 17%
• UpdateSim time: 0.003ms

DART

Static

Before this PR:

• RTF: 12.5%
• UpdateSim time: 1.1ms

After this PR:

• RTF: 15%
• UpdateSim time: .011ms

Non-Static

Before this PR:

• RTF: 1.1%
• UpdateSim time: 2.8ms

After this PR:

• RTF: 1.1%
• UpdateSim time: 1.7ms

Takeaways

• Static model performance for TPE is near optimal now - RTF is close to 100% when running headless, as expected for static models with TPE.
• DART does not benefit from these changes as much as TPE because the Step in DART still accounts for most of the work being done in a physics cycle.
• Why is there still a yellow Models profiler block for non-static models with DART? -> For non-static models with DART, some models are still processed as changed due to contact forces (this isn't an issue for TPE). If gravity is set to 0 for a world that uses DART, then no models are perceived as changed. I have found that around 25-50% of models that are affected by contact forces in DART are registered as "changed" by ign-physics, but we are still better off only updating a subset of the models than all of them in UpdateSim.
• There is a chance that the performance gains won't be as noticeable for a world with a lot of non-static objects that change often. The reason for this is because we are now returning a std::map instead of a std::unordered_map in the ChangedLinks method in order to guarantee topological ordering of links (consider a nested model where each model has its own canonical link - we still want to make sure the parent model is updated first, so we must process the parent's canonical link first in order to preserve topological ordering/update behavior). Because of this change, inserting changed links into this map is now of time complexity O(log(n)) instead of O(1). However, we still have the advantage of only iterating through the changed links in UpdateSim to update models instead of updating through every model, so I don't believe that the changes in this PR would cause any noticeable performance decreases (especially since the use case of a lot of non-static models that move often is probably rare anyways). In fact, if we look at the non-static test case for DART above (where 25-50% of the 3000 models are still being processed as "changed" due to contact forces, which means we are inserting into this map often), UpdateSim time still improved by about 1ms.

Future work

Next, I'd like to work on #711. After this, it would be worth investigating performance when using a GUI (so far, all testing has been done headless). I spoke with @iche033 offline, and we found that the RTF between headless and GUI simulation results in significantly different RTF numbers when using the changes in this PR (on my machine, I saw that RTF dropped from 80%-30% once the GUI was populated for the 3000 static shapes test with TPE). We also found that RTF drops significantly (from 90% to 10% for me running headless, 3000 static shapes with TPE) when echoing the /world/shapes/dynamic_pose/info topic). Here are a few more issues that I created so that I can address them as I continue to work on performance improvements:

• Performance drops significantly when echoing the /world/shapes/dynamic_pose/info topic #743
• Improve GUI performance #744

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]

Codecov Report

Merging #736 (fb4a177) into ign-gazebo5 (d3ee64a) will increase coverage by 0.08%.
The diff coverage is 88.88%.

❗ Current head fb4a177 differs from pull request most recent head 48acd0e. Consider uploading reports for the commit 48acd0e to get more accurate results

@@               Coverage Diff               @@
##           ign-gazebo5     #736      +/-   ##
===============================================
+ Coverage        65.32%   65.41%   +0.08%     
===============================================
  Files              240      241       +1     
  Lines            17624    17650      +26     
===============================================
+ Hits             11513    11545      +32     
+ Misses            6111     6105       -6     

Impacted Files	Coverage Δ
src/systems/physics/Physics.cc	69.44% <83.01%> (ø)
src/systems/physics/EntityFeatureMap.hh	93.65% <90.90%> (ø)
...lude/ignition/gazebo/components/ReferenceModels.hh	100.00% <100.00%> (ø)
src/SdfEntityCreator.cc	87.43% <100.00%> (+0.20%)	⬆️
src/SimulationRunner.cc	93.77% <0.00%> (+1.03%)	⬆️

---

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 dca877a...48acd0e. Read the comment docs.

[iche033]

nice I also see a good increase in RTF. For static models in TPE, I get 30% on my laptop and 55% on desktop.

Just some minor comments but changes look good to me

[adlarkin]

After some discussion that was had offline, I have moved this new canonical link mapping functionality into a class internal to the physics system in 24ebe9e. This approach has the following advantages:

• We do not need to introduce a new component to help achieve this functionality (originally, I created a ReferenceModels component which had the potential for introducing complications later on: Map canonical links to their models #736 (comment))
• We can leave the SdfEntityCreator untouched, and also no longer need to deprecate the CanonicalLink or ModelCanonicalLink components since we are still using them (Map canonical links to their models #736 (comment))
• If we ever need to change how this mapping is implemented, we can do so without breaking API or ABI since this is now internal to the physics system plugin

The topological ordering works here based on the assumption that entities are created from parent to child, with entity IDs created in ascending order (notice how links are stored in a std::map and how models are stored in a std::set since each of these data structures preserve ascending order). This should mean that behavior isn't broken for users who create entities with or without the SdfEntityCreator (i.e., there's no implicit dependency on the SdfEntityCreator). I believe that the only edge case here would be if a user created a child link/model entity before creating the parent, because then, the child entity ID would be less than the parent entity ID, but is this even possible with our current API? cc @azeey

---

All tests still pass for me, and I also re-ran performance tests to verify that this still helps TPE performance. This should now be ready for final review!

[iche033]

latest changes look good to me.

[chapulina]

LGTM, I just have a couple of comments.

[chapulina]

Looks great! Feel free to merge with happy CI ✔️

[azeey]

I believe that the only edge case here would be if a user created a child link/model entity before creating the parent, because then, the child entity ID would be less than the parent entity ID, but is this even possible with our current API?

This is certainly possible if users create link/model entities without going through the SdfEntityCreator. They can first create a link, then the model and set the model as the parent of the link. Of course, this is not a common use case, so I'm good with merging this PR. But perhaps we can leave #706 open until we implement the topological ordering by using parent-child relationships encoded by the ParentEntity component or the relationships maintained by the graph in the ECM.

[adlarkin]

perhaps we can leave #706 open until we implement the topological ordering by using parent-child relationships encoded by the ParentEntity component or the relationships maintained by the graph in the ECM

Sounds good to me 👍 I updated the PR description so that this doesn't close #706. I also went ahead and left a comment in #706 that links to the discussion we had here to give others an update: #706 (comment)

I'm just waiting for all checks to run one last time, but if they come back green, I'll merge this.