Cache top level and static to speed up physics system

[chapulina]

🦟 Bug fix

Summary

Profiling a world as follows:

• 3000 non-static simple shapes sitting on a static ground plane
• running with unbounded RTF (-z 1000000000)
• running headless (with -s and only the Physics system)
• Using TPE as the physics engine

I noticed that the PhysicsPrivate::UpdateSim was taking way too long for what it does. All that function is doing is getting resulting data from the ign-physics step and populating the ECM, and that takes ~70% of an iteration:

Before:

I think there's a lot of room for improvement there, but here are just a couple of little improvements. Caching a couple of values that never change (top-level model and static), instead of reevaluating them at every iteration, we can reduce that to closer to 60%:

After:

For static, one thing to consider is leveraging the ECM and adding Static components to links that are static, so that the Each call can iterate only over static links.

Checklist

• Signed all commits for DCO
• Added tests
• 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 #656 (407614a) into ign-gazebo4 (302f5ed) will decrease coverage by 12.61%.
The diff coverage is 59.17%.

@@               Coverage Diff                @@
##           ign-gazebo4     #656       +/-   ##
================================================
- Coverage        77.37%   64.76%   -12.62%     
================================================
  Files              217      232       +15     
  Lines            12217    16601     +4384     
================================================
+ Hits              9453    10751     +1298     
- Misses            2764     5850     +3086     

Impacted Files	Coverage Δ
include/ignition/gazebo/EntityComponentManager.hh	100.00% <ø> (ø)
include/ignition/gazebo/rendering/SceneManager.hh	100.00% <ø> (ø)
.../plugins/component_inspector/ComponentInspector.cc	6.11% <0.00%> (-2.50%)	⬇️
.../plugins/component_inspector/ComponentInspector.hh	28.57% <ø> (ø)
src/gui/plugins/scene3d/Scene3D.cc	8.72% <0.00%> (ø)
src/systems/diff_drive/DiffDrive.hh	100.00% <ø> (ø)
src/systems/sensors/Sensors.cc	74.75% <0.00%> (ø)
src/rendering/SceneManager.cc	22.92% <8.79%> (ø)
src/systems/joint_controller/JointController.cc	77.17% <12.50%> (-6.16%)	⬇️
...int_position_controller/JointPositionController.cc	71.29% <12.50%> (-4.71%)	⬇️
... and 38 more

---

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 8e29cda...407614a. Read the comment docs.

[nkoenig]

This worked great on SubT. Performance went from 65-70% to around 95% when running a single robot in a world.

[adlarkin]

Changes seem okay to me. It seems like there wasn't much of an improvement if I ran this with the 3000 shapes world with static models instead of non-static, which was a little surprising to me. But I think that this is a step in the right direction, and we will probably need to continue making changes like this.

Also, I didn't see any issues in the code, but I'm not too familiar with the physics system, so if someone else wants to take a look at the changes as a sanity check, that might be a good idea (I think @nkoenig may have already done this).