Optimize vectorization of free-floating Jacobian

[diegoferigo]

Currently we are computing the free-floating jacobian $J \in \mathbb{R}^{6 \times (6 + n)}$ using the following algorithm:

jaxsim/src/jaxsim/physics/algos/jacobian.py

Lines 14 to 25 in 4fd2032

	def jacobian(model: PhysicsModel, body_index: jtp.Int, q: jtp.Vector) -> jtp.Matrix:
	"""
	Compute the Jacobian matrix for a specific link in an articulated body or robot.
	Args:
	model (PhysicsModel): The physics model of the articulated body or robot.
	body_index (jtp.Int): The index of the link for which to compute the Jacobian matrix.
	q (jtp.Vector): Joint positions (Generalized coordinates).
	Returns:
	jtp.Matrix: The Jacobian matrix for the specified link.
	"""

When jit-compiled, body_index must be a static argument, therefore the following vectorization is quite slow:

jaxsim/src/jaxsim/api/model.py

Lines 460 to 469 in 4fd2032

	J_free_floating = jax.vmap(
	lambda i: to_output(
	W_J_WL=js.link.jacobian(
	model=model,
	data=data,
	link_index=i,
	output_vel_repr=VelRepr.Inertial,
	)
	)
	)(jnp.arange(model.number_of_links()))

We should investigate if we can compute a "full" free-floating jacobian (with all columns filled) in a shared representation (like a doubly-left one), and then produce the link jacobian by filtering the columns using the support parent array $\kappa(i)$ together with appropriate conversions of input/output velocity representations.

[diegoferigo]

I implemented the algorithm to compute the doubly-left full Jacobian ${}^B J_{W,\text{\_}/B}$ in #121. Closing.