Add utilities to handle SDF models as a directed trees

[diegoferigo]

This PR introduces a number of new features, all targeted to be used as utilities for processing a rod.Model as a tree. Some of the feature below or future features are taken from jaxsim.parsers, that could be replaced in the future by these new rod features.

1. Creating directed trees

Although SDFormat supports closed kinematic loops / parallel robots (example), rod currently doesn't, as by the way also URDF. Excluding these particular families of kinematics, any robot model could be modelled as a directed tree (i.e. an oriented Direct Acyclic Graph with nodes restricted to have just one parent).

In our directed tree:

• Links are the nodes
• Joints are the edges

Furthermore, in order to support SDF frames, we also allow attaching extra elements to both links and joints.

2. Computing forward kinematics

The new TreeTransforms class allows to compute the forward kinematics of links, frames, and joints. The pose of these elements can be expressed wrt any other element part of the rod.Model.

Currently this FK is meant to be used just for performing computations on the kinematic tree, that means that we consider all joints having a zero positions. This limitation will be removed in future PRs.

3. Resolving frames

A SDF model could have either explicit or implicit frames. To ease processing, this PR introduces a resolve_model_frames helper that adds to all elements of the model an explicit pose.

4. Switching frame convention

Partially related to feature 3, SDF has its own default convention of implicit frames when either the pose element is missing or the relative_to attribute is not set. This PR introduces a switch_frame_convention helper that updates all transforms following different frame conventions:

• Sdf: converts all rod.Pose elements to use the default reference frames of SDF
• Model: converts all rod.Pose elements to be expressed wrt their parent model (__model__)
• World: converts all rod.Pose elements to be expressed wrt the world frame
• Urdf: converts all rod.Pose elements to be expressed wrt the equivalent defaults used in URDF

This is particularly useful especially for the last convention, for example if in the future we want to convert a deserialized SDF to URDF since serializing the XML having already the correct poses would be pretty easy.

Future work

This PR is already useful as it is and I propose to merge it with these features that are already too many. Future PR will address some features left out:

• Implementation of lumping links together (this PR only supports lumping a link with zero inertial properties, e.g. a URDF frame)¹
• Implement a proper forward kinematics that allows to specify joint positions
• Use the two features above to enable model reduction, i.e. removing a set of joints (possibly specifying their default position) and properly lump the removed links into the ones that remain ²

Footnotes

1. could be taken from jaxsim.parser.descriptions.LinkDescription.lump_with like other features of this PR ↩

2. could be taken from jaxsim.parsers.KinematicGraph.reduce ↩