Test J̇ computation against AD

[diegoferigo]

This PR extends the test included in #169 to compare the computation of $\dot{J}$ with the corresponding derivative computed as:

$$\dot{J}(\mathbf{q},\, \boldsymbol{\nu}) = \frac{\text{d} J(\mathbf{q})}{\text{d} t} = \frac{\partial J}{\partial \mathbf{q}} \frac{\partial \mathbf{q}}{\partial t} = \frac{\partial J}{\partial \mathbf{q}} \dot{\mathbf{q}}$$

As usual, it's worth noting that since $\mathbf{q} = ( {}^W \mathbf{p}_B \,; {}^W \mathtt{Q}_B\,; \mathbf{s} ) \in \mathbb{R}^{7+n}$, we have that $\dot{\mathbf{q}} \neq \boldsymbol{\nu}$. However, by taking extra care of the derivative of the base quaternion, we can compute $\dot{\mathbf{q}}$ and use it in the test (that, btw, is independent from the velocity representation).

Triggered by #169 (comment) from @DanielePucci.

---

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

[diegoferigo]

Out of curiosity, I've tried to benchmark on CPU the two implementations:

	Analytical $\dot{J}$	AD $\dot{J}$
JIT compilation	2.08 s	4.04 s
Runtime	248 µs ± 18.5 µs	6.65 ms ± 633 µs

This numbers refer to the computation of $\dot{J}$ of a single link of the ErgoCub robot (57 DoFs, pretty large). It seems pretty clear that the analytical computation has always to be preferred.

[diegoferigo]

This could be interesting to all @ami-iit/vertical_control-oriented-learning.

[flferretti]

This is great! Thanks Diego

[flferretti]

I think I'm missing something, can we just compare the two ${} ^O\dot{J} _{WL, I}$?

Suggested change

	assert jnp.einsum("l6g,g->l6", O_J̇_ad_WL_I, I_ν) == pytest.approx(
	jnp.einsum("l6g,g->l6", O_J̇_WL_I, I_ν)
	)
	assert O_J̇_ad_WL_I == pytest.approx(O_J̇_WL_I)

[diegoferigo]

The problem is that the elements of $\dot{J}$ can be really close to $0$. Computing the link bias acceleration, instead, provides larger values since they sum up along kinematic chains.

I think it's more robust having also the projected values checked. I fear that comparing the Jacobians with too small numbers may provide a 0 = 0 even if the values are wrong (but so small that are within the default tolerances of pytest). And, I don't want to mess us with the tolerances. If something related to them is wrong, the last assert will fail.

[flferretti]

Great, thanks for the explanation