Support specifying link pose wrt parent joint

[diegoferigo]

Currently, as inheritance of the kinematics formulated by the RBDA book, our physics assumes that the link frames are positioned exactly over the successor frame of their parent joint.

This means that all SDF models having a non-trivial /link/pose are not yet supported. Note that this does not constrains in any way model creation, in fact, any non-supported model could be edited without a big effort to combine the pose of child elements of the links (inertial, visual, collisions, ...) with the unsupported link pose.

<link name="link">
  <!-- This has to be zero. If not, it has to be combined with the pose of the child elements below -->
  <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
  <inertial>
    <!-- This pose has to be combined with the link pose if the link pose is not zero -->
    <!--pose>0.0 0.0 0.0 0.0 0.0 0.0</pose-->
    <mass>1.0</mass>
    <inertia>
      <ixx>0.025</ixx>
      <iyy>0.025</iyy>
      <izz>0.025</izz>
    </inertia>
  </inertial>
  <visual name="one_visual">
    <!-- This pose has to be combined with the link pose if the link pose is not zero -->
    <!--pose>0.0 0.0 0.0 0.0 0.0 0.0</pose-->
    <geometry>
      <box>
        <size>0.1 0.1 0.1</size>
      </box>
    </geometry>
  </visual>
</link>

In any case, this limitation has to be solved from our side. It is not trivial, since we need to treat differently the successor frame of a joint and the frame of its child link.

[diegoferigo]

For the records, the currently implemented behavior is compatible with URDF requirements, therefore if the SDF loaded into jaxsim is converted from any URDF, the resulting model is automatically compatible with jaxsim.

xref gazebosim/sdformat#273 (comment)

[diegoferigo]

The new joint model introduced with the functional APIs in #108 (more specifically, #101) should now support the definition of a successor-to-child transform ${}^{\text{suc}} \mathbf{H}_{\text{i}}$. This transform is initialized to store the optional link pose w.r.t. their parent joint (only supported in SDF and not in URDF).

In particular, we initialize this transform here:

jaxsim/src/jaxsim/math/joint_model.py

Lines 101 to 105 in e917034

	suc_H_i = suc_H_i.at[joint.index].set(
	description.relative_transform(
	relative_to=joint.name, name=joint.child.name
	)
	)

And then we combine it to compute the overall parent-to-child transform ${}^i \mathbf{H}_{\lambda(i)}(s)$ here:

jaxsim/src/jaxsim/api/kin_dyn_parameters.py

Lines 401 to 407 in e917034

	# Extract the successor-to-child fixed transforms.
	# Note that here we include also the index 0 since suc_H_child[0] stores the
	# optional pose of the base link w.r.t. the root frame of the model.
	# This is supported by SDF when the base link <pose> element is defined.
	suc_H_i = jax.vmap(lambda i: self.joint_model.successor_H_child(joint_index=i))(
	jnp.arange(0, 1 + self.number_of_joints())
	)

We should write a test making sure that this feature is implemented as expected, but for the time being we can consider this issue as solved.