Prototype system for initializing model state

[scpeters]

🎉 New feature

Closes #2318

Summary

This adds a system called SetModelState that accepts an xml configuration containing <model_state> tags intending to serve as a prototype for new syntax for the <state> tag in SDFormat 1.12. Currently, only <joint_state> tags are supported; support should also be added for <link_state>.

Example <model_state> syntax from examples/worlds/set_model_state.sdf

• Using degrees

        <model_state>
          <joint_state name="j1">
            <axis_state>
              <position degrees="true">60</position>
              <velocity degrees="true">-30</velocity>
            </axis_state>
          </joint_state>
        </model_state>

• Using radians

        <model_state>
          <joint_state name="j1">
            <axis_state>
              <position>1.047</position>
              <velocity>-0.523</velocity>
            </axis_state>
          </joint_state>
        </model_state>

Test it

gz sim -v 4 examples/worlds/set_model_state.sdf

The rotors should start at a 60 degree angle and rotate clock-wise at 30 degrees / second.

Checklist

• [ ] Signed all commits for DCO
• [ ] 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)
• [ ] 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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[scpeters]

For specifying link velocity, there has been a request to change the link_state specification for velocity to not use the pose type (see https://github.com/gazebosim/sdformat/issues/1366). As mentioned in that issue, an alternative is to specify velocity as two vector3 types since there is no vector6 type in SDFormat and this avoids confusion about the order of the data.

<model_state>
  <link_state name="link1">
    <linear_velocity>{vx} {vy} {vz}</linear_velocity>
    <angular_velocity>{wx} {wy} {wz}</angular_velocity>
  </link_state>
</model_state>

[traversaro]

Can this be used to set the model state of a model in which no gz-sim-set-model-state-system, for example to permit to have a generic model for a robot, that is include in a .world and in which world-specific positions are specified?

[scpeters]

Can this be used to set the model state of a model in which no gz-sim-set-model-state-system, for example to permit to have a generic model for a robot, that is include in a .world and in which world-specific positions are specified?

Yes, I would like to push these syntax changes upstream to the the <state> specification in SDFormat 1.12. I'm treating this pull request and system as a prototype for those syntax changes.

[scpeters]

One difference between this prototype and what we could achieve by updating the //world/state specification is that this plugin allows specifying the initial state of a model from within the //model element, whereas the //world/state element must be defined at the world level. I think it is convenient to set model state from within the model, so I would also propose adding a //model/state tag as well, which would add complexity and require defining an order or precedence, but I think it would be a valuable addition.

[scpeters]

I've updated the world file to use SDFormat 1.11 and auto-inertials in https://github.com/gazebosim/gz-sim/pull/2359/commits/7cab1bb44ece6d20986c9832afee436b5c3a69d2