$$\nu\times\phi=\BIN v\\\omega\BOUT\times\BIN f\\\tau\BOUT=\BIN\omega\times f \\\omega\times\tau+ v \times f \BOUT$$

A special form of the motion-force product is often used:

\begin{align*}\nu\times (Y \nu) &= \nu\times\BIN mv - mc\times\omega\\ mc\times v + I \omega - mc\times(c\times\omega) \BOUT\\&= \BIN m \omega\times v - \omega\times(mc\times\omega) \\\omega\times ( mc \times v) + \omega\times (I\omega) -\omega\times(c \times( mc\times\omega)) -v\times(mc \times\omega)\BOUT\end{align*}

Setting $\beta=mc \times\omega$, this product can be written:

$$\nu\times(Y \nu)=\BIN\omega\times(m v -\beta)\\\omega\times( c \times(mv-\beta)+I\omega)- v \times\beta\BOUT$$

This last form cost five $\times$, four $+$ and one $3\times3$ matrix-vector multiplication.

\section{Joint}

We denote by $1$ the coordinate system attached to the parent (predecessor) body at the joint input, ad by $2$ the coordinate system attached to the (child) successor body at the joint output. We neglect the possible time variation of the joint model (ie the bias velocity $\sigma=\nu(q,0)$ is null).

The joint geometry is expressed by the rigid transformation from the input to the ouput, parametrized by the joint coordinate system $q \in\mathcal{Q}$:

$$^2m_1\repr\ ^2M_1(q)$$

The joint velocity (i.e. the velocity of the child wrt. the parent in the child coordinate system) is:

$$^2\nu_{12}=\nu_J(q,v_q)=\ ^2S(q) v_q $$

where $^2S$ is the joint Jacobian (or constraint matrix) that define the motion subspace allowed by the joint, and $v_q$ is the joint coordinate velocity (i.e. an element of the Lie algebra associated with the joint coordinate manifold), which would be $v_q=\dot q$ when $\dot q$ exists.

The joint acceleration is:

$$^2\alpha_{12}= S \dot v_q + c_J +\ ^2\nu_{1}\times\ ^2\nu_{12}$$

where $c_J =\sum_{i=1}^{n_q}\dpartial{S}{q_i}\dot q_i$ (null in the usual cases) and $^2\nu_{1}$ is the velocity of the parent body with respect to an absolute (Galilean) coordinate system\footnote{The abosulte velocity $\nu_{1}$ is also the relative velocity wrt. the Galilean coordinate system $\Omega$. The exhaustive notation should be $\nu_{\Omega1}$ but $\nu_1$ is prefered for simplicity.}.

The joint calculations take as input the joint position $q$ and velocity $v_q$ and should output $^2M_1$, $^2\nu_{12}$ and $^2c$ (this last vector being often a trivial $0_6$ vector). In addition, the joint model should store the position of the joint input in the central coordinate system of the previous joint $^0m_1$ which is a constant value.

The joint integrator computes the exponential map associated with the joint manifold. The function inputs are the initial position $q_0$, the velocity $v_q$ and the length of the integration interval $t$. It computes $q_t$ as:

$$ q_t = q_0+\int_0^t v_q dt$$

For the simple vectorial case where $v_q=\dot q$, we have $q_t=q_0+ t v_q$. Written in the more general case of a Lie groupe, we have $q_t = q_0 exp(t v_q)$ where $exp$ denotes the exponential map (i.e. integration of a constant vector field from the Lie algebra into the Lie group). This integration only consider first order explicit Euler. More general integrators (e.g. Runge-Kutta in Lie groupes) remains to be written. Adequate references are welcome.

\section{RNEA}

\subsection{Initialization}

$$^0\nu_0=0 ; \ ^0\alpha_0=-g$$

In the following, the coordinate system $i$ is attached to the output of the joint (child body), while $lambda(i)$ is the central coordinate system attached to the parent joint. The coordinated system associated with the joint input is denoted by $i_0$. The constant rigid transformation from $\lambda(i)$ to the joint input is then $^{\lambda(i)}M_{i_0}$.

\subsection{Forward loop}

For each joint $i$, update the joint calculation $\mathbf j_i$.calc($q,v_q$). This compute $\mathbf{j}.M =\ ^{\lambda(i)}M_{i_0}(q)$, $\mathbf{j}.\nu=\ ^i\nu_{{\lambda(i)}i}(q,v_q)$, $\mathbf{j}.S =\ ^iS(q)$ and $\mathbf{j}.c =\sum_{k=1}^{n_q}\dpartial{^iS}{q_k}\dot q_k$. Attached to the joint is also its placement in body $\lambda(i)$ denoted by $\mathbf{j}.M_0=\ ^{\lambda(i)}M_{i_0}$. Then:

It is more efficient to apply $X^{-1}$ than $X$. Similarly, it is more efficient to apply $X^{-*}$ than $X^*$. Therefore, it is better to store the transformations $^{\lambda(i)}m_i$ and $^0m_i$ than $^im_{\lambda(i)}$ and $^im_0$.