mujoco: Optimize WorldForwardStep Output

[iche033]

🎉 New feature

Addresses #299
Depends on:

• mujoco: Initial implementation #811
• mujoco: Add support for GetContactsFromLastStep feature #889

Summary

Updated mujoco's WorldForwardStep implementation to write ChangedWorldPoses to ForwardStep::Output. Previously it was writing all link poses instead of only ones that have changed.

Note that one difference with the dartsim plugin is that in mujoco I loop through all the worlds and track previous link poses for each world. So the prevLinkPoses variable is now stored in WorldInfo struct. Technically it's not necessary since we only support one world at the moment.

Test it

ctest -R simulation_features_mujoco

Checklist

• Signed all commits for DCO
• Added a screen capture or video to the PR description that demonstrates the feature
• Added tests
• Added example and/or tutorial
• 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)
• Updated Bazel files (if adding new files). Created an issue otherwise.
• While waiting for a review on your PR, please help review another open pull request to support the maintainers
• Was GenAI used to generate this PR? If so, make sure to add "Generated-by" to your commits. (See this policy for more info.)

Generated-by: Remove this if GenAI was not used.

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by and Generated-by messages.

Backports: If this is a backport, please use Rebase and Merge instead.