iterate through changed links only in UpdateSim

[adlarkin]

Signed-off-by: Ashton Larkin [email protected]

🎉 New feature

requires:

• write modified link data to output in physics step gz-physics#223
• Add "ChangedWorldPose" to ForwardStep::Output's expected data gz-physics#238
• add Equal tolerance method to Quaternion gz-math#196

related to gazebosim/gz-physics#219

Summary

As mentioned in gazebosim/gz-physics#219, UpdateSim in the ign-gazebo physics system iterates over all links, because it has no way of knowing which links have moved. gazebosim/gz-physics#223 and gazebosim/gz-physics#238 should resolve this issue, so this PR uses each of these ign-physics PRs (and gazebosim/gz-math#196) to have UpdateSim iterate over just the links that have moved.

As a result of this change, the link pose caching that was done in #669 is no longer needed if a physics engine is being used that writes output pose data (we still need the caching done in #656 regardless of whether the physics engine being used writes output pose data or not). I also had to add some functionality to the EntityFeatureMap class (introduced in #586) that maps physics link entity IDs (std::size_t) to the corresponding entity in ign-gazebo (the link data that ign-gazebo gets from ign-physics contains a link's pose and link ID).

This new behavior is optional. In other words, if a physics engine is being used that writes output pose data, then the output pose data will be used to update the simulation state. If a physics engine is being used that does not write output pose data, then the approach for updating the simulation state remains unchanged (getting all link poses from the latest step via ECM).

Test it

Testing this PR can follow the testing approach taken in #656 and #669, which involves profiling a world as follows:

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

Profiling the world above can be done with both DART and TPE physics engines. In the comparisons below, the metrics being compared to (i.e., before this PR) are being done on commit 2bf29c2 of ign-gazebo, and commit gazebosim/gz-physics@54d2a44 of ign-physics.

TPE

Static

Before this PR:

• RTF: 21%
• UpdateSim time: 3ms

After this PR:

• RTF: 40%
• UpdateSim time: 0.67ms

Non-Static

Before this PR:

• RTF: 4.8%
• UpdateSim time: 13.6ms

After this PR:

• RTF: 13.8%
• UpdateSim time: 0.8ms

DART

Static

Before this PR:

• RTF: 8.5%
• UpdateSim time: 4.1ms

After this PR:

• RTF: 12.5%
• UpdateSim time: 1.1ms

Non-Static

Before this PR:

• RTF: 0.95%
• UpdateSim time: 15.6ms

After this PR:

• RTF: 1.1%
• UpdateSim time: 2.8ms

Takeaways

The TPE performance improvement is noticeable (2x RTF increase for static shapes, and 3x RTF increase for non-static shapes). The performance improvement for DART is not as noticeable (1.5x RTF increase for static shapes, and no real change in RTF for non-static shapes), probably because UpdateSim is a small part of the Update in the physics system - for DART, most of the work in the physics system Update comes from the physics step itself, which we probably don't have a ton of control over when it comes to optimization.

Future Work

Although there are noticeable performance improvements here, there are a few things that I'd like to do next to create even more performance improvements:

1. address Add a topologically sorted list of models to the physics system #706 - if you take a look at the profiler images above, you'll notice that most of the time in UpdateSim is taken up by Models, which is a loop that updates model poses if necessary. Since we now only have the links that have changed after each physics step, it would be nice to make a bi-directional mapping between models and their canonical links (currently, we only have a mapping from a model to its canonical link - see Support individual canonical links for nested models #685). That way, we can loop through all of the changed links and update the models that have this link as its canonical link instead of looping through all of the models and seeing if a model's canonical link has changed pose. I believe that this would decrease the Model block time significantly.
2. optimize the EntityComponentManager::Each call. As I mentioned in iterate through changed links only in UpdateSim #678 (comment), reducing the number of components used in the ecm.Each call sped up the time for the ecm.Each call to complete by approximately 3x (wow!). Considering how often we use ecm.Each in ign-gazebo, I imagine that optimizing this function call would result in a massive performance benefit. I have created an issue for this so that it can be tracked: Improve the performance of EntityComponentManager::Each #711

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

[azeey]

At a high level, my main concern is that this changes the requirement on the ign-physics plugin such that the ForwardStep::World::Step function now has to return the world poses. We have been trying not to require features in a released version and this feels like we're breaking that. I'd like to hear @chapulina's thoughts on this.

A possible workaround to maintain backward compatibility would be to populate stepOutput in ign-gazebo if the return of ForwardStep::World::Step doesn't contain any world poses.

[chapulina]

We have been trying not to require features in a released version

+1, we should be mindful of not introducing new required APIs that may break existing 3rd party physics engines.

possible workaround to maintain backward compatibility

That sounds like a good idea.

Another thing to consider is working on this feature on top of Edifice so that we're free to change requirements and implement the feature in the ideal way. Once that's settled, we can think of how to backport it.

[adlarkin]

Thanks for the comments, @azeey and @chapulina. I am going to update this PR and gazebosim/gz-physics#223 to target Edifice, and then we can determine the next best steps.

[adlarkin]

This is now ready for review, and targets Edifice (main branch).

As I mentioned in the PR description, this change eliminates the need for #669, which is something to keep in mind when we make the next forward-port.

We'll need to make a minor release for ign-math6 so that gazebosim/gz-math#196 can be used.

[adlarkin]

As I mentioned in the PR description, this change eliminates the need for #669, which is something to keep in mind when we make the next forward-port.

I just updated this branch to include the changes in #695, and resolved conflicts to use the changes in this PR instead of the changes in #669: 1074b5a

[azeey]

LGTM!

[azeey]

@osrf-jenkins run tests please

[adlarkin]

LGTM!

I'm making one last change that will make this new physics output optional (if poses aren't a part of ignition::physics::ForwardStep::Output, then the physics system will iterate through all of the links in UpdateSim instead of through the links in ignition::physics::ForwardStep::Output). Once that change is in, I believe this can be merged.

[adlarkin]

Conflicts have been resolved in dca0f66, but it looks like there's a new test failure that I need to resolve (INTEGRATION_wind_effects). Using physics engine output to determine which links changed is still optional behavior (if a physics engine doesn't write output, the ECM is used like we've been doing up until this point).

@azeey, I had to make a few changes to the physics system when resolving merge conflicts in order to support the work you did in #685. The main difference that was made was including a modelWorldPoses cache since the frame data that is used in UpdateSim only contains links that have changed instead of all links (which means that we don't update every model's pose every time, so we should cache model poses to avoid losing pose information for models that aren't updated). Also, as we discussed offline, I made some optimizations to the model pose update ecm.each call in 2571923.

To give more insight into the benefits of 2571923, here are the profiler results before this commit using the same testing methodology described in the PR description (static models world with TPE). As we can see, the ecm.Each model loop takes 2.5ms:

After applying the changes made in 2571923, the ecm.Each model loop takes 0.79ms:

It's interesting to see how much faster the ecm.Each call can be when it is used with fewer components (in this case, we only use the components we absolutely need with ecm.Each, and then query other components later on if we need them). Perhaps optimizing ecm.Each is something I can look into as I continue to work on simulation performance updates.

---

Testing the optional behavior

To revert to the old behavior of using the ECM to go through all links for updates instead of using physics output for updating links, make the following changes in ign-physics (this is assuming you're on commit b10a4c3 in ign-physics):

diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc
index 2c4ef89..e7aa863 100644
--- a/dartsim/src/SimulationFeatures.cc
+++ b/dartsim/src/SimulationFeatures.cc
@@ -59,7 +59,7 @@ void SimulationFeatures::WorldForwardStep(
 
   // TODO(MXG): Parse input
   world->step();
-  this->Write(_h.Get<ChangedWorldPoses>());
+  //this->Write(_h.Get<ChangedWorldPoses>());
   // TODO(MXG): Fill in state
 }
 
diff --git a/tpe/plugin/src/SimulationFeatures.cc b/tpe/plugin/src/SimulationFeatures.cc
index 30db7b4..10992c3 100644
--- a/tpe/plugin/src/SimulationFeatures.cc
+++ b/tpe/plugin/src/SimulationFeatures.cc
@@ -62,7 +62,7 @@ void SimulationFeatures::WorldForwardStep(
     }
   }
   world->Step();
-  this->Write(_h.Get<ChangedWorldPoses>());
+  //this->Write(_h.Get<ChangedWorldPoses>());
 }

You can also add some print statements inside the if...else block of the PhysicsPrivate::ChangedLinks method in the physics system to get further validation on which method is being used for recording link updates.

[chapulina]

Removing from beta, this can come in a minor release.

[adlarkin]

it looks like there's a new test failure that I need to resolve (INTEGRATION_wind_effects)

I fixed the failure in 4f44bfc. The issue was that since we are now only updating the simulation if a pose change greater than 1e-6 took place (check in ign-physics if the engine writes output and check in ign-gazebo if the physics engine doesn't write output), the object wasn't moving fast enough after applying the wind force in the test. As a result, it took several iterations after applying the wind force for the object to undergo a pose change greater than 1e-6, so an update wasn't registered until this point, and this check failed:

ign-gazebo/test/integration/wind_effects.cc:325: Failure
Expected: (std::fabs(lastVelMagnitude - velMagnitude)) < (1e-6), actual: 6.34714e-05 vs 1e-06

[  FAILED  ] WindEffectsTest.TopicsAndServices (90 ms)
[----------] 4 tests from WindEffectsTest (1445 ms total)

[----------] Global test environment tear-down
[==========] 4 tests from 1 test suite ran. (1445 ms total)
[  PASSED  ] 3 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] WindEffectsTest.TopicsAndServices

 1 FAILED TEST

I increased the amount of wind force applied so that the object has a higher velocity, which results in more frequent physics updates after the wind force is no longer applied. This ensures that we don't see any velocity jumps after applying the wind force in the INTEGRATION_wind_effects test.

[adlarkin]

I have addressed all review feedback and updated this PR with the latest updates from the main branch. All tests and codecheck pass for me locally. I believe this is ready for final review!

[azeey]

Great work!!

[codecov]

Codecov Report

❗ No coverage uploaded for pull request base (ign-gazebo5@0802698). Click here to learn what that means.
The diff coverage is n/a.

❗ Current head d3ee64a differs from pull request most recent head 1924de9. Consider uploading reports for the commit 1924de9 to get more accurate results

@@              Coverage Diff               @@
##             ign-gazebo5     #678   +/-   ##
==============================================
  Coverage               ?   65.32%           
==============================================
  Files                  ?      240           
  Lines                  ?    17624           
  Branches               ?        0           
==============================================
  Hits                   ?    11513           
  Misses                 ?     6111           
  Partials               ?        0           

---

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 0802698...1924de9. Read the comment docs.

[adlarkin]

Now that Edifice has been released, should I change the base branch to ign-gazebo5 before merging?