Extend JaxSimModelReferences to support applying forces to frames

[diegoferigo]

We already have JaxSimModelReferences.apply_link_forces to apply forces to the links. Now that frame support is finalized (#147), it would be great adding a new JaxSimModelReferences.apply_frame_force method that allows to apply a 6D force to a given frame. In this way, the logic can directly detect the parent link of the frame and automatically express the 6D frame force to the frame of the parent link.

[flferretti]

Given a frame force ${} _F F$ expressed in inertial representation, the corresponding force on the parent link will be given by:

$${} _L F = {} _L X ^F {} _F F$$

in which:

$$\begin{align} {} _L X ^F &= (\text{Adj} {} ^L H _F) ^{-\top} \\\ {} ^L H _F &= {} ^L H _W {} ^W H _F = ({} ^W H _L) ^{-1} {} ^W H _F \end{align}$$

This logic can be parallelized to obtain the tuples $\verb |(link_force_i, parent_link_idx_i)|$ that will define the terms of a force summation on the links.

[diegoferigo]

I want to extend a bit the math since I'm reviewing #209 and there's something that might be not 100% correct.

Let's consider the following two frames:

1. A link frame $L$.
2. A generic frame $F$ rigidly attached to link $L$ and having a fixed pose defined by ${}^L \mathbf{H}_F$.

Let's now consider a 6D force $\mathbf{f}$ applied to the origin of frame $F$. The aim of the desired helper is to express such force to the link frame $L$. Since our internal buffers store velocites and forces in inertial-fixed representation, we need to also convert the frame force to inertial-fixed.

To be as generic as possible, let's consider an input 6D force expressed in frame $Y \in \left[ W, F, F[W] \right]$ that describes inertial-fixed, body-fixed, and mixed representations.

What we need to do is the following:

$${}_L \mathbf{f} = {}_L \mathbf{X}^Y {}_Y \mathbf{f}$$

where ${}_L \mathbf{X}^Y = {}^Y \mathbf{X}_L^\top = {}^L \mathbf{X}_Y^{-\top} $.

[flferretti]

Thanks a lot @diegoferigo! I've updated my comment accordingly