Fix TPE Link velocity not being updated and Model velocity not having any effect.

[luca-della-vedova]

🦟 Bug fix

Summary

This PR fixes two separate issues regarding TPE:

The first one was likely introduced in the performance optimization PRs (such as #223). Velocity control of models using TPE now doesn't work anymore. To try it out compare the results of:
ign gazebo /home/luca/ws_edifice/src/ign-gazebo/examples/worlds/velocity_control.sdf --physics-engine ignition-physics-tpe-plugin
and
ign gazebo /home/luca/ws_edifice/src/ign-gazebo/examples/worlds/velocity_control.sdf
The green vehicle does not move under TPE.
My suspicion is that it is due to the fact that instead of sending all the links we now send only updated links and since the velocity is updated at the model level the links themselves are not updated.
The fix I came up with involves iterating over the models in the update step (skipping static models) and, when a model had its pose updated, add all its links to the output of Step so that Gazebo will correctly update them. This fix makes the example work again, I'm not sure about the performance overhead but hopefully because of the static check it shouldn't be too large for worlds with a lot of static objects.

The second issue has probably always been there. As part of ign-gazebo #966, I have been trying to implement velocity control using LinearVelocity and AngularVelocity components, however it seems velocity was always read as 0 in TPE.
I noticed that in KinematicsFeature velocity is updated for models. However, in the physics update step we only send link information, not model information.
As a consequence I decided to also calculate and send Link velocity information in the global frame, this fixes the ign-gazebo issue (that can be tested with the luca/linVelCmdTest in ign-gazebo), as far as TPE is concerned.

I only managed to add a test for the second issue, a simple velocity loopback that fails before the PR and succeeds afterwards, I think the test for the last issue is a bit more complex since it involves the data being passed to ign-gazebo.

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 #289 (1f949e4) into ign-physics4 (f38120b) will increase coverage by 0.10%.
The diff coverage is 96.77%.

@@               Coverage Diff                @@
##           ign-physics4     #289      +/-   ##
================================================
+ Coverage         75.75%   75.86%   +0.10%     
================================================
  Files               125      125              
  Lines              5503     5531      +28     
================================================
+ Hits               4169     4196      +27     
- Misses             1334     1335       +1     

Impacted Files	Coverage Δ
tpe/plugin/src/SimulationFeatures.cc	82.71% <95.83%> (+4.38%)	⬆️
tpe/plugin/src/KinematicsFeatures.cc	70.00% <100.00%> (+9.13%)	⬆️

---

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 f38120b...1f949e4. Read the comment docs.

[iche033]

nice, thanks for catching this. Confirms that the vehicles now move with vel cmds. Got a comment about potential duplicate link entries and vector size.

[luca-della-vedova]

Fixed both the comments here. I changed the variable name to prevEntityPoses and added a unordered_set containing the id of links that have already been pushed to the vector to avoid adding duplicated entries.

[iche033]

looks good to me.

[adlarkin]

Thanks for the fixes, @luca-della-vedova! From what I recall with my recent testing on DART, these issues did not exist on DART - I believe that they only existed with TPE. Did you by chance test these things with DART to make sure that these issues do not exist with DART? If there are no issues with DART, then I believe we can close gazebosim/gz-sim#966 once we get some feedback on gazebosim/gz-sim#966 (comment).