$$\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$.
where \f$c_J = \sum_{i=1}^{n_q} \dpartial{S}{q_i} \dot q_i\f$ (null in the usual cases) and \f$^2\nu_{1}\f$ is the
velocity of the parent body with respect to an absolute (Galilean) coordinate system\footnote{The abosulte velocity
\f$\nu_{1}\f$ is also the relative velocity wrt. the Galilean coordinate system \f$\Omega\f$. The exhaustive notation
should be \f$\nu_{\Omega1}\f$ but \f$\nu_1\f$ is prefered for simplicity.}.
The joint calculations take as input the joint position \f$q\f$ and velocity \f$v_q\f$ and should output \f$^2M_1\f$,
\f$^2\nu_{12}\f$ and \f$^2c\f$ (this last vector being often a trivial \f$0_6\f$ vector). In addition, the joint model
should store the position of the joint input in the central coordinate system of the previous joint \f$^0m_1\f$ which is a constant value.
The joint integrator computes the exponential map associated with the joint manifold. The function inputs are the
initial position \f$q_0\f$, the velocity \f$v_q\f$ and the length of the integration interval \f$t\f$. It computes \f$q_t\f$ as:
\f$ q_t = q_0 + \int_0^t v_q dt\f$
For the simple vectorial case where \f$v_q=\dot q\f$, we have \f$q_t=q_0 + t v_q\f$. Written in the more general case of a Lie groupe, we have \f$q_t = q_0 exp(t v_q)\f$ where \f$exp\f$ 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.
## RNEA
### Initialization
\f$^0\nu_0 = 0 ; \ ^0\alpha_0 = -g\f$
In the following, the coordinate system \f$i\f$ is attached to the output of the joint (child body), while \f$lambda(i)\f$ is the central coordinate system attached to the parent joint. The coordinated system associated with the joint input is denoted by \f$i_0\f$. The constant rigid transformation from \f$\lambda(i)\f$ to the joint input is then \f$^{\lambda(i)}M_{i_0}\f$.
### Forward loop
For each joint \f$i\f$, update the joint calculation \f$\mathbf j_i\f$.calc(\f$q,v_q\f$). This compute \f$\mathbf{j}.M = \ ^{\lambda(i)}M_{i_0}(q)\f$, \f$\mathbf{j}.\nu = \ ^i\nu_{{\lambda(i)}i}(q,v_q)\f$, \f$\mathbf{j}.S = \ ^iS(q)\f$ and \f$\mathbf{j}.c = \sum_{k=1}^{n_q} \dpartial{^iS}{q_k} \dot q_k\f$. Attached to the joint is also its placement in body \f$\lambda(i)\f$ denoted by \f$\mathbf{j}.M_0 =\ ^{\lambda(i)}M_{i_0}\f$. Then:
It is more efficient to apply \f$X^{-1}\f$ than \f$X\f$. Similarly, it is more efficient to apply \f$X^{-*}\f$ than \f$X^*\f$. Therefore, it is better to store the transformations \f$^{\lambda(i)}m_i\f$ and \f$^0m_i\f$ than \f$^im_{\lambda(i)}\f$ and \f$^im_0\f$.
