Compute the pose and the jacobian of the implicit frames associated to collidable points

[diegoferigo]

We consider the following implicit frame associated to each collidable point:

$$C := \left({}^W \mathbf{p}_C, [L]\right)$$

where:

• ${}^W \mathbf{p}_C \in \mathbb{R}^3$ is the position of the collidable point in world coordinates.
• $[L]$ is the orientation frame of the parent link of the collidable point (i.e. the link it is rigidly attached to).

This PR implements the following:

• The world-to-contact-frame transform ${}^W \mathbf{H}_C \in \text{SE}(3)$.
• The free-floating jacobian ${}^O J_{W,C/I}$ of the contact frame, where $I$ is the input representation that should match the representation of $\boldsymbol{\nu} \in \mathbb{R}^{6+n}$, and $O$ the desired output representation.

---

📚 Documentation preview 📚: https://jaxsim--163.org.readthedocs.build//163/

[xela-95]

Is this correct? I was expecting it to be ${} ^{C[W]} J _{W, C}$ 🤔

[diegoferigo]

Yep you're right, that's the mixed velocity and I should have named the variable CW_J_WC. Thanks!

[flferretti]

Great! Thanks Diego, LGTM