devel_guide/notes/NewtonEuler.tex

\begin{tabular}{lll}
  \centering
  Author &  O. Bonnefon &2010\\
  Revision& section \ref{Sec:NE_motion} to \ref{Sec:NE_TD} V. Acary&  05/09/2011\\
  Revision& section \ref{Sec:NE_motion}  V. Acary&  01/06/2016\\
  Revision& complete edition V. Acary&  06/01/2017\\

\end{tabular}

\def\glaw{\cdot}
\def\cg{\sf \small g}

\section{The equations of motion}


In the maximal coordinates framework, the most natural choice for the kinematic  variables and for the formulation of the equations of motion is the Newton/Euler formalism, where the equation of motion describes the translational and rotational dynamics of each body using a specific choice of parameters. For the translational motion, the position of the center of mass $x_{\cg}\in \RR^3$ and its velocity  $v_{\cg} = \dot x_{\cg} \in \RR^3$ is usually chosen. For the orientation of the body is usually defined by the rotation matrix $R$ of the body-fixed frame with respect to a given inertial frame.

For the rotational motion, a common choice is to choose the rotational velocity  $\Omega \in \RR^3$ of the body expressed in the body--fixed frame. This choice comes from the formulation of a rigid body motion of a point $X$ in the inertial frame as
\begin{equation}
  \label{eq:1}
  x(t) = \Phi(t,X) = x_{\cg}(t) + R(t) X.
\end{equation}
The velocity of this point can be written as
\begin{equation}
  \label{eq:2}
  \dot x(t) = v_{\cg}(t) + \dot R(t) X
\end{equation}
Since $R^\top R=I$, we get $R^\top \dot R + \dot R^\top R =0$. We can conclude that it exists a matrix $\tilde \Omega := R^\top \dot R $ such that $\tilde \Omega + \tilde \Omega^\top=0$, i.e. a skew symmetric matrix. The notation $\tilde \Omega$ comes from the fact that there is a bijection between the skew symmetric matrix in $\RR^{3\times3}$ and $\RR^3$ such that
\begin{equation}
  \label{eq:3}
  \tilde \Omega x  = \Omega \times x, \quad \forall x\in \RR^3.
\end{equation}
The rotational velocity is then related to the $R$ by :
\begin{equation}
  \label{eq:angularvelocity}
  \widetilde \Omega = R^\top \dot R, \text { or equivalently, } \dot R  = R \widetilde \Omega
\end{equation}

Using these coordinates, the equations of motion are given by
\begin{equation}
  \label{eq:motion-NewtonEuler}
  \left\{\begin{array}{rcl}
      m \;\dot v_{\cg}  & = &f(t,x_{\cg}, v_{\cg},  R,  \Omega) \\
      I \dot \Omega + \Omega \times I \Omega &= & M(t,x_{\cg}, v_{\cg}, R, \Omega) \\
      \dot x_{\cg}&=& v_{\cg}\\
      \dot R  &=& R \widetilde \Omega
    \end{array}
  \right.
\end{equation}
where $m> 0$ is the mass, $I\in \RR^{3\times 3}$ is the matrix of moments of inertia around the center of mass and the axis of the body--fixed frame.

The vectors $f(\cdot)\in \RR^3$ and $M(\cdot)\in \RR^3$ are the total forces and torques applied to the body. It is important to outline that the total applied forces $f(\cdot)$ has to be expressed in a consistent frame w.r.t. to $v_{\cg}$. In our case, it hae to be expressed in the inertial frame. The same applies for the moment $M$ that has to be expressed in the body-fixed frame. If we consider a moment $m(\cdot)$ expressed in the inertial frame, then is has to be convected to  the body--fixed frame thanks to
\begin{equation}
  \label{eq:convected_moment}
  M (\cdot) =R^\top  m (\cdot)
\end{equation}


\begin{remark}
If we perform the time derivation of $RR^\top =I$ rather than $R^\top R=I$, we get $R \dot R^\top + \dot R R^\top =0$.  We can conclude that it exists a matrix $\tilde \omega := \dot R R^\top $ such that $\tilde \omega + \tilde \omega^\top=0$, i.e. a skew symmetric matrix. Clearly, we have
 \begin{equation}
   \label{eq:4}
   \tilde \omega = R \tilde \Omega R^\top
 \end{equation}
 and it can be proved that is equivalent to $ \omega =R \Omega$. The vector $\omega$ is the rotational velocity expressed in the inertial frame. The equation of motion can also be expressed in the inertial frame as follows
  \begin{equation}
  \label{eq:motion-NewtonEuler-inertial}
  \left\{\begin{array}{rcl}
      m \;\dot v_{\cg}  & = &f(t,x_{\cg}, v_{\cg},  R,  R^T \omega) \\
      J(R) \dot \omega + \omega \times J(R) \omega &= & m(t,x_{\cg}, v_{\cg}, R, \omega) \\
      \dot x_{\cg}&=& v_{\cg}\\
      \dot R  &=& \widetilde \omega R
    \end{array}
  \right.
\end{equation}
where the matrix $J(R) = R I R^T$ is the inertia matrix in the inertial frame.
Defining the angular momentum with respect to the inertial frame as
\begin{equation}
  \label{eq:1}
  \pi(t) = J(R(t)) \omega(t)
\end{equation}
the equation of the angular motion is derived from the balance equation of the angular momentum
\begin{equation}
  \label{eq:5}
  \dot \pi(t) = m(t,x_{\cg}, v_{\cg}, R, \omega)).
\end{equation}

\end{remark}

For a given constant (time invariant) $\tilde \Omega$, let us consider the differential equation
\begin{equation}
  \label{eq:5}
  \begin{cases}
    \dot R(t) = R \tilde \Omega\\
    R(0) = I
  \end{cases}
\end{equation}
Let us recall the definition of the matrix exponential,
\begin{equation}
  \label{eq:6}
  \exp(A) = \sum_{k=0}^{\infty} \frac {1}{k!} A^k
\end{equation}
A trivial solution of \eqref{eq:5} is $R(t) = \exp(t\tilde\Omega) $ since
\begin{equation}
  \label{eq:7}
  \frac {d}{dt}(\exp(At)) = \exp(At) A.
\end{equation}
More generally, with the initial condition $R(t_0)= R_0$, we get the solution
\begin{equation}
R(t) = R_0 \exp((t-t_0)\tilde\Omega)\label{eq:8}
\end{equation}

Another interpretation is as follows. From a (incremental) rotation vector, $\Omega$ and its associated matrix $\tilde \Omega$, we obtain a rotation matrix by the exponentation of $\tilde \Omega$:
\begin{equation}
  \label{eq:9}
  R = \exp(\tilde\Omega).
\end{equation}
Since we note that $\tilde \Omega^ 3 = - \theta^2 \tilde \Omega$ with $\theta = \|\Omega\|$, it is possible to get a closed form of the matrix exponential of $\tilde \Omega$
\begin{equation}
  \label{eq:10}
  \begin{array}[lcl]{lcl}
    \exp(\tilde \Omega) &=& \sum_{k=0}^{\infty} \frac {1}{k!} (\tilde \Omega)^k \\
                        &=&  I_{3\times 3} + \sum_{k=1}^{\infty} \frac {(-1)^{k-1}}{(2k-1)!}  \theta ^{2k-1} \tilde \Omega + (\sum_{k=0}^{\infty} \frac {(-1)^{k-1}}{(2k)!} \theta)^{2k-2} \tilde \Omega^2\\[2mm]
                        &=&  I_{3\times 3} + \frac{\sin{\theta}} {\theta} \tilde \Omega +  \frac{(\cos{\theta}-1)}{\theta^2}\tilde \Omega^2
  \end{array}
\end{equation}
that is
\begin{equation}
  \label{eq:11}
  R =  I_{3\times 3} + \frac{\sin{\theta}} {\theta} \tilde \Omega +  \frac{(\cos{\theta}-1)}{\theta^2}\tilde \Omega^2
\end{equation}
The formula \eqref{eq:11} is the Euler--Rodrigues formula that allows to compute the rotation matrix on closed form.


\begin{ndrva}
  todo :
  \begin{itemize}
  \item add the formulation in the inertial frame of the Euler
    equation with $\omega =R \Omega$.
  \item Check that \eqref{eq:10} is the Euler-Rodrigues formula and not the Olinde Rodrigues formula. (division by $\theta$)
\end{itemize}

\end{ndrva}

In the numerical practice, the choice of the rotation matrix is not convenient since it introduces redundant parameters. Since $R$ must belong to $SO^+(3)$, we have also to satisfy $\det(R)=1$ and $R^{-1}=R^\top$. In general, we use a reduced vector of parameters $p\in\RR^{n_p}$ such $R = \Phi(p)$ and $\dot p = \psi(p)\Omega $. We denote  by $q$ the vector of coordinates of the position and the orientation of the body, and by $v$ {the body twist}:
\begin{equation}
  q \coloneqq \begin{bmatrix}
    x_{\cg}\\
    p
  \end{bmatrix},\quad
  v \coloneqq \begin{bmatrix}
     v_{\cg}\\
     \Omega
   \end{bmatrix}.
 \end{equation}
 The relation between $v$ and the time derivative of $q$ is
\begin{equation}
  \label{eq:TT}
  \dot q =
  \begin{bmatrix}
     \dot x_{\cg}\\
     \psi(p) \dot p
   \end{bmatrix}
   =
   \begin{bmatrix}
     I & 0 \\
     0 & \psi(p)
   \end{bmatrix}
   v
   \coloneqq
   T(q) v
\end{equation}
with $T(q) \in \RR^{{3+n_p}\times 6}$.
{Note that the twist $v$ is not directly the time derivative of the coordinate vector as a major difference with Lagrangian systems. }

%
The Newton-Euler equation in compact form may be written as:
\begin{equation}
\label{eq:Newton-Euler-compact}
\boxed{ \left \{
 \begin{aligned}
  &\dot q=T(q)v, \\
  & M \dot v = F(t, q, v)
 \end{aligned}
 \right.}
\end{equation}
where $M\in\RR^{6\times6}$ is the total inertia matrix
\begin{equation}
  M:= \begin{pmatrix}
    m I_{3\times 3} & 0 \\
    0 & I
  \end{pmatrix},
\end{equation}
and $F(t, q, v)\in \RR^6$ collects all the forces and torques applied to the body
\begin{equation}
  F(t,q,v):= \begin{pmatrix}
    f(t,x_{\cg},  v_{\cg}, R, \Omega ) \\
    I \Omega \times \Omega + M(t,x_{\cg}, v_{\cg}, R, \Omega )
  \end{pmatrix}.
\end{equation}
When a collection of bodies is considered, we will use the same notation as in~(\ref{eq:Newton-Euler-compact}) extending the definition of the variables $q,v$ and the operators $M,F$ in a straightforward way.


\input{LieGroupTheory.tex}
\section{ Lie group $SO(3)$ of finite rotations and Lie algebra $\mathfrak{so}(3)$ of infinitesimal rotations}
The presentation is this section follows the notation and the developments taken from~\cite{Iserles.ea_AN2000,Munthe-Kaas.BIT1998}. For more details on Lie groups and Lie algebra, we refer to \cite{Varadarajan_book1984} and \cite{Helgason_Book1978}.


The Lie group $SO(3)$ is the group of linear proper orthogonal transformations in $\RR^3$ that may be represented by a set of matrices in $\RR^{3\times 3}$ as
\begin{equation}
  \label{eq:47}
  SO(3) = \{R \in \RR^{3\times3}\mid R^TR=I , det(R) = +1  \}
\end{equation}
with the group law given by $R_1\glaw R_2 = R_1R_2$ for $R_1,R_2\in SO(3)$. The identity element is $e = I_{3\times 3}$. At any point of $R\in SO(3)$, the tangent space $T_RSO(3)$ is the set of tangent vectors at a point $R$.

\paragraph{Left representation of  the tangent space at $R$, $T_RSO(3)$ } Let $S(t)$ be a smooth curve $S(\cdot) : \RR  \rightarrow SO(3)$ in $SO(3)$. An element $a$ of the tangent space at $R$ is given by
\begin{equation}
  \label{eq:174}
  a  = \left.\frac{d}{dt} S(t)\right|_{t=0}
\end{equation}
such that $S(0)= R$.
Since $S(t)\in SO(3)$, we have  $\frac{d}{dt} (S(t)) = \dot S(t)S^T(t) +  S(t) \dot S^T(t) =0$. At $t=0$, we get $a R^T +  R a^T =0$.
We conclude that it exists a skew--symmetric matrix $\tilde \Omega = R^T a$ such that $\tilde \Omega^T + \tilde \Omega =0$. Hence, a possible representation of  $T_RSO(3)$ is
\begin{equation}
  \label{eq:49}
  T_RSO(3) = \{ a = R \tilde \Omega \in \RR^{3\times 3} \mid \tilde \Omega^T + \tilde \Omega =0 \}.
\end{equation}
For $R=I$, the tangent space is directly given by the set of  skew--symmetric matrices:
\begin{equation}
  \label{eq:50}
  T_ISO(3) = \{ \tilde \Omega\in \RR^{3\times 3} \mid \tilde \Omega^T + \tilde \Omega =0 \}.
\end{equation}
The tangent space $T_ISO(3)$ with the Lie Bracket $[\cdot,\cdot]$ defined by the matrix commutator
\begin{equation}
  \label{eq:51}
  [A,B] = AB-BA
\end{equation}
is a Lie algebra that is denoted by
\begin{equation}
  \label{eq:53}
  \mathfrak{so}(3) =\{\Omega\in \RR^{3\times 3} \mid \Omega + \tilde \Omega^T =0\}.
\end{equation}
 For skew symmetric matrices, the commutator can be expressed with the cross product in $\RR^3$
\begin{equation}
  \label{eq:52}
  [\tilde \Omega, \tilde \Gamma] = \tilde \Omega \tilde \Gamma - \tilde \Gamma \tilde \Omega= \widetilde{\Omega \times \Gamma }
\end{equation}
We use   $T_ISO(3) \cong  \mathfrak{so}(3)$ whenever there is no ambiguity.

The notation $\tilde \Omega$ is implied by the fact that the Lie algebra is isomorphic to $\RR^3$ thanks to the operator $\widetilde{(\cdot)} :\RR^3 \rightarrow \mathfrak{so}(3)$ and defined by
\begin{equation}
  \label{eq:54}
 \widetilde{(\cdot)}: \Omega \mapsto \tilde \Omega =
  \begin{bmatrix}
    0 & -\Omega_3 & \Omega_2 \\
    \Omega_3 & 0 & -\Omega_1 \\
    -\Omega_2  & \Omega_1 & 0
  \end{bmatrix}
\end{equation}
Note that $\tilde \Omega x = \Omega \times x$.

\paragraph{ A special  (right)  action of Lie Group $\mathcal G$ on a manifold $\mathcal M$. }

Let us come back to the representation of  $T_RSO(3)$ given in~\eqref{eq:49}. It is clear it can expressed with a representation that relies on $\mathfrak{so}(3)$
\begin{equation}
  \label{eq:58}
   T_RSO(3) = \{ a = R \tilde \Omega \in \RR^{3\times 3} \mid \tilde \Omega \in \mathfrak{so}(3) \}.
\end{equation}
With \eqref{eq:58}, we see that there is a linear map that relates $T_RSO(3)$ to  $\mathfrak{so}(3)$. This can be formalize by noting that the left translation map for a point $R \in SO(3)$
\begin{equation}
  \label{eq:59}
  \begin{array}[lcl]{rcl}
    L_R& :&   SO(3)  \rightarrow  SO(3)\\
       & &  S  \mapsto L_R(S) = R \glaw S = RS\\
  \end{array}
\end{equation}
which is diffeomorphism on $SO(3)$ is a group action. In our case, we identify the manifold and the group. Hence, the mapping $L_R$ can be viewed as a left or a right group action. We choose a right action such that $\Lambda^r(R,S) = L_{R}(S) =  R \glaw S $. By differentiation, we get a mapping $L'_R: T_I\mathfrak{so(3)} \cong \mathfrak{so(3)} \rightarrow T_R SO(3)$. For a given $\tilde\Omega \in \mathfrak{so(3)}$ and a point $R$, the differential $L'_R$ by computing the tangent vector field $\lambda^r_{*}(a)(R)$ of the group action  $\Lambda^r(R,S)$ for a smooth curve $S(t) : \RR \rightarrow S0(3)$ such that $\dot S(0) = \tilde\Omega$:
\begin{equation}
  \label{eq:60}
   \lambda^r_{*}(a)(R) \coloneqq  \left. \frac{d}{dt} \Lambda^r(R,S(t)) \right|_{t=0} = \left. \frac{d}{dt} L_{R}(S(t)) \right|_{t=0} =  \left. \frac{d}{dt} R \glaw S(t) \right|_{t=0} =  R \glaw \dot S(0) = R \tilde\Omega \in X(\mathcal M)
 \end{equation}
%
Therefore, the vector field in \eqref{eq:60} is a tangent vector field that defines a Lie-Type ordinary differential equation
\begin{equation}
  \label{eq:61}
  \dot R(t) = \lambda^r_{*}(a)(R(t)) = R(t)  \tilde \Omega
\end{equation}


In~\cite{Bruls.Cardona2010}, the linear operator $\lambda^r_{*}(a)$  is defined as  the directional derivative with respect to $S$ an denoted $DL_R(S)$. It defines a diffeomorphism between $T_SSO(3)$ and $T_{RS}SO(3)$. In particular, for $S=I_{3\times3}$, we get
\begin{equation}
  \label{eq:62}
  \begin{array}{rcl}
    DL_R(I_{3\times3}) : \mathfrak{so}(3) & \rightarrow & T_R SO(3) \\
    \tilde \Omega &\mapsto &DL_R(I_{3\times3}). \tilde \Omega = R \tilde \Omega
  \end{array}
\end{equation}
We end up with a possible representation of $T_{R} SO(3)$ as
\begin{equation}
  \label{eq:63}
  T_{R} SO(3) =\{\tilde \Omega_R \mid \tilde \Omega_R = DL_R(I_{3\times3}). \tilde \Omega = R \tilde \Omega, \tilde \Omega \in\mathfrak{so}(3)  \}.
\end{equation}
In other words, a tangent vector $\tilde \Omega \in \mathfrak{so}(3)$ defines a left invariant vector field on $SO(3)$ at the point $R$ given by $R \tilde \Omega$.


\begin{ndrva}
  what happens at $S(0)=R$, with $ a =R \tilde \Omega =\dot S(0)$ and then $\dot y(t) = F(y(t)) = R \tilde \Omega y(t) =  R\Omega \times y(t)= \dot S(0) y(t) $. What else ?
\end{ndrva}


\paragraph{Exponential map $\expm \mathfrak{so(3)} \rightarrow SO(3)$}
The relations \eqref{eq:24} and \eqref{eq:25} shows that is possible to define tangent vector field from a group action. We can directly apply Theorem~\ref{Theorem:solutionofLieODE} and we get that the solution of
\begin{equation}
  \label{eq:130}
  \begin{cases}
  \dot R(t) = \lambda^r_{*}(a)(R(t)) = R(t)  \tilde \Omega \\
  R(0) = R_0
\end{cases}
\end{equation}
 is
\begin{equation}
  \label{eq:138}
  R(t) = R_0 \expm(t \tilde \Omega)
\end{equation}

Let us do the  computation in this case. Let us assume that the solution can be sought as $R(t) = \Lambda^r(y_o,S(t))$. The initial condition imposes that  $R(0) = R_0 = \Lambda(R_0,I) = \Lambda(R_0,S(0))$ that implies $S(0)=I$. Since $\Lambda(R_0,S(t))$ is the flow that is produces by $S(t)$ and let us try to find the relation satisfied by $S(\cdot)$. For a smooth curve $T(s) \in SO(3)$ such that $\dot T(0)= \tilde \Omega$, we have
\begin{equation}
  \label{eq:64}
  \begin{array}[lcl]{lcl}
    \dot R(t) = \lambda^r_*(\tilde \Omega)(R(t)) &=& \left. \frac{d}{ds}\Lambda^r(R(t),T(s)) \right|_{s=0} \\
                                &=& \left. \frac{d}{ds} \Lambda^r(\Lambda(R_0, S(t)),T(s)) \right|_{s=0} \\
                                &=& \left. \frac{d}{ds} (\Lambda^r(R_0, S(t)\glaw T(s)) \right|_{s=0} \\
                                &=& D_2 \Lambda^r(R_0, \glaw S(t) \glaw \dot T(0) ) \\
                                &=& D_2 \Lambda^r(R_0,  S(t)\glaw \tilde \Omega )
  \end{array}
\end{equation}
On the other side, the relation $y(t) = \Lambda^r(y_0,S(t))$ gives $\dot y(t) = D_2 \Lambda^r(y_0,S'(t))$ and we conclude that
\begin{equation}
  \label{eq:65}
  \begin{cases}
    \dot S(t) =  S(t)\glaw\tilde\Omega    = S(t) \tilde \Omega\\
    S(0) = I.
  \end{cases}
\end{equation}
The ordinary differential equation~\eqref{eq:65} is a matrix ODE that admits the following solution
\begin{equation}
  \label{eq:66}
  S(t) = \expm(t\tilde\Omega)
\end{equation}
where $\exp : \RR^{3\times 3} \rightarrow \RR^{3\times 3}$ is the matrix exponential defined by
\begin{equation}
  \label{eq:67}
  \begin{array}[lcl]{lcl}
    \expm(A) &=& \sum_{k=0}^{\infty} \frac {1}{k!} (A)^k.
  \end{array}
\end{equation}
We conclude that $R(t) =\Lambda(R_0,S(t)) = R_0\expm(t\tilde\Omega)$ is the solution of \eqref{eq:35}.

We can use the closed form solution for the matrix exponential of $t \tilde\Omega  \in \mathfrak{so}(3)$ as
\begin{equation}
  \label{eq:68}
  \expm(t\tilde\Omega) = I_{3\times 3} + \frac{\sin{t\theta}} {\theta}  \tilde\Omega  +  \frac{(\cos{t \theta}-1)}{\theta^2} \tilde\Omega^2
\end{equation}
with $\theta = \|\Omega\|$.
For given  $\tilde \Omega \in\mathfrak{so}(3)$, we have
\begin{equation}
  \label{eq:69}
  \det(\tilde \Omega) = \det(\tilde \Omega^T) = \det (-\tilde \Omega^T) = (-1)^3 \det(\tilde \Omega ) = - \det (\tilde \Omega )
\end{equation}
that implies that $\det(\tilde \Omega ) =0 $. From \eqref{eq:68}, we conclude that
\begin{equation}
  \label{eq:70}
  \det( \expm(t\tilde \Omega)) = 1.
\end{equation}
Furthermore, we have $\expm(t\tilde \Omega)\expm( -t\tilde \Omega) = \expm(t(\tilde \Omega-\tilde \Omega)) = I$. We can verify  that  $\expm(t\tilde \Omega) \in SO(3)$.

\paragraph{Adjoint representation}
In the case of $SO(3)$, the definition of the operator $\Ad$ gives
\begin{equation}
  \label{eq:121}
  \Ad_R(\tilde\Omega)  = R \tilde\Omega R^T
\end{equation}
 and then mapping $\ad_{\tilde\Omega}(\tilde \Gamma)$ is defined by
\begin{equation}
  \label{eq:56}
  \ad_{\tilde\Omega}(\tilde\Gamma) = \tilde \Omega \tilde\Gamma - \tilde \Gamma \tilde\Omega  =  [\tilde \Omega,\tilde \Gamma] = \widetilde{\Omega \times \Gamma}.
\end{equation}
Using the isomorphism between $\mathfrak so(3)$ and $\RR^3$, it possible the define the mapping $\ad_{\Omega}(\Gamma) : \RR^3\times\RR^3 \rightarrow \RR^3$ with the realization of the Lie algebra in $\RR^3$ as
\begin{equation}
  \label{eq:55}
  \ad_\Omega(\Gamma) = \tilde\Omega \Gamma = \Omega\times\Gamma
\end{equation}

\paragraph{Differential of the exponential map $\dexpm$}
The differential of the exponential mapping, denoted by $\dexpm$ is defined as the 'right trivialized' tangent of the exponential map
\begin{equation}
  \label{eq:71}
  \frac{d}{dt} (\exp(\tilde \Omega(t))) = \dexp_{\tilde\Omega(t)}(\frac{d \tilde{\Omega}(t)}{dt}) \exp(\tilde\Omega(t))
\end{equation}


% \begin{ndrva}
%   explain briefly the notion of left-invariant vector field
% \end{ndrva}
%\href{https://en.wikipedia.org/wiki/Lie_group}{https://en.wikipedia.org/wiki/Lie_group}
%Finally, the straight line $\alpha \tilde \Omega$ for $\Omega$

The differential of the exponential mapping, denoted by $\dexpm$ is defined as the 'left trivialized' tangent of the exponential map
\begin{equation}
  \label{eq:72}
   \frac{d}{dt} (\exp(\tilde \Omega(t))) = \dexp_{\tilde\Omega(t)}(\frac{d \tilde{\Omega}(t)}{dt}) \exp(\tilde\Omega(t))
\end{equation}

Using the formula~\eqref{eq:43} and the fact that $\ad_\Omega(\Gamma) = \Tilde\Omega \Gamma$, we can write the differential as
\begin{equation}
  \label{eq:122}
  \begin{array}{lcl}
    \dexp_{\tilde\Omega}(\tilde\Gamma) &=& \sum_{k=0}^\infty \frac{1}{(k+1)!} \ad_{\tilde \Omega}^k (\tilde\Gamma) \\
                                       &=& \sum_{k=0}^\infty \frac{1}{(k+1)!} \tilde\Omega^k \tilde \Gamma \\
  \end{array}
\end{equation}
Using again the fact that $\tilde\Omega^3 = -\theta^2 \tilde\Omega$, we get
\begin{equation}
  \label{eq:123}
   \begin{array}{lcl}
     \dexp_{\tilde\Omega} &=& \sum_{k=0}^\infty  \frac{1}{(k+1)!} \tilde\Omega^k \\
                          &=& I  + \sum_{k=0}^\infty  \frac{(-1)^k}{((2(k+1))!} \theta^{2k} \tilde\Omega + \sum_{k=0}^\infty  \frac{(-1)^k}{((2(k+1)+1)!} \theta^{2k} \tilde\Omega^2\\
  \end{array}
\end{equation}
Hence, we get
\begin{equation}
  \label{eq:124}
   \begin{array}{lcl}
     \dexp_{\tilde\Omega}  &=& I  + \frac{(1-\cos(\theta))}{\theta^2}\tilde\Omega + \frac{(\theta-\sin(\theta))}{\theta^3}\tilde\Omega^2
  \end{array}
\end{equation}
Since $\dexp_{\tilde\Omega}$ is a linear mapping from $\mathfrak{so(3)}$ to $\mathfrak{so(3)}$, we will use the following notation
\begin{equation}
  \label{eq:172}
  \dexp_{\tilde\Omega}\tilde\Gamma  \coloneqq T(\Omega)\tilde\Gamma
\end{equation}
with
\begin{equation}
  \label{eq:173}
   T(\Omega) \coloneqq I  + \frac{(1-\cos(\theta))}{\theta^2}\tilde\Omega + \frac{(\theta-\sin(\theta))}{\theta^3}\tilde\Omega^2  \in \RR^{3\times 3}
\end{equation}


\subsection{Newton method and differential of a map $f : \mathcal G \rightarrow \mathfrak g$}
Finally, let us define the differential of the map $f : SO(3) \rightarrow \mathfrak {so}(3)$ as
\begin{equation}
  \label{eq:183}
  \begin{array}[rcl]{rcl}
    f'_R : T_RSO(3) &\rightarrow&T_{f(R)}\mathfrak {so}(3) \cong  \mathfrak {so}(3)\\
           a &\mapsto& \left.\frac{d}{dt} f(R\glaw \expm(t L'_{R^{-1}}(a))) \right|_{t=0}
  \end{array}
\end{equation}
The image of $b$ by $f'_z$   is obtained by first identifying $a$ with an element of $\tilde\Omega \in \mathfrak {so}(3)$ thanks to the left representation of $T_{f(R)}\mathfrak {so}(3)$ view the left translation map $\tilde\Omega= t L'_R(b)$. The exponential mapping transforms $\tilde\Omega$ an element $S$ of the Lie Group $SO(3)$. Then $f'_z$ is obtained by
\begin{equation}
  \label{eq:184}
  f'_R(b) = \lim_{t\rightarrow 0} \frac{f(R\glaw S) - f(R)}{t}
\end{equation}
As we have done for the exponential mapping, it is possible to get a left trivialization of
\begin{equation}
  \label{eq:185}
  \dd f_R = (f\circ L_R)' = f'_R \circ L'_R
\end{equation}
thus
\begin{equation}
  \label{eq:186}
  \dd f_R (\tilde\Omega) =  f'_R \circ L'_R(\tilde\Omega) = f'_R(L'_R(\tilde\Omega)) =  \left.\frac{d}{dt} f(R\glaw \expm(t \tilde\Omega )) \right|_{t=0}
\end{equation}

\begin{ndrva}
  The computation of this differential is non linear with respect to $\tilde\Omega$.


  not clear if we write $\dd f_R (\tilde\Omega)$. Better understand the link with $\dexp_{\tilde \Omega}{\tilde\Gamma}$
\end{ndrva}
Sometimes, it can be formally written as
\begin{equation}
  \label{eq:180}
  \dd f_R (\tilde\Omega) = C(\tilde\Omega)\tilde\Omega
\end{equation}
Nevertheless, an explicit expression of $C(\cdot)$ is not necessarily trivial.

Let us consider a first simple example of a mapping $f(R) = \widetilde{R  x}$ for a given $x\in\RR^3$. The computation yields
\begin{equation}
  \label{eq:181}
  \begin{array}{rcl}
    \dd f_R (\tilde\Omega) &=& \widetilde{ \left.\frac{d}{dt} R \exp(t \tilde\Omega) x  \right|_{t=0}} \\
                           &=& \widetilde{R \left.\frac{d}{dt}\exp(t \tilde\Omega)\right|_{t=0}  x} \\
                           &=& \widetilde{R \left. \dexp_{\tilde\Omega}(\tilde\Omega)\exp(t \tilde\Omega) \right|_{t=0}  x} \\
                           &=& \widetilde{R \dexp_{\tilde\Omega}(\tilde\Omega) x} \\
                           &=& \widetilde{R T(\Omega) \tilde\Omega  x} \\
                           &=& \widetilde{-R T(\Omega) \tilde x \Omega }
  \end{array}
\end{equation}
In that case, it is difficult to find a expression as in \eqref{eq:180}, but considering the function $g(R)$ such that $f(R) = \widetilde g(x)$ we get
\begin{equation}
  \label{eq:181}
  \begin{array}{rcl}
    \dd g_R (\tilde\Omega)  =- R T(\Omega) \tilde x \Omega  = C(\Omega) \Omega
  \end{array}
\end{equation}
with
\begin{equation}
  \label{eq:182}
   C(\Omega) = -R T(\Omega) \tilde x
\end{equation}


\section{Lie group of unit quaternions $\HH_1$ and pure imaginary quaternions $\HH_p$.}


In Siconos we choose to parametrize the rotation with a unit quaternion $p \in \HH$ such that $R = \Phi(p)$. This parameterization has no singularity and has only one redundant variable that is determined by imposing $\|p\|=1$.


\paragraph{Quaternion definition.} There is many ways to define quaternions. The most convenient one is to define a quaternion as
a $2\times 2$ complex matrix, that is an element of $\CC^{2\times 2}$. For this end, we write for $z \in \CC$, $z=a+ib$ with $a,b \in \RR^2$ and $i^2=-1$ and its conjugate $\bar z= a-ib$. Let ${e, \bf, i, j, k}$ the following matrices in $\CC^{2\times 2}$
\begin{equation}
  \label{eq:127}
  e =
  \begin{bmatrix}
    1 & 0 \\
    0 & 1  \\
  \end{bmatrix},
  \quad   \bf{i} =
  \begin{bmatrix}
    i & 0 \\
    0 & -i  \\
  \end{bmatrix}
  \quad   \bf{j} =
  \begin{bmatrix}
    0 & 1 \\
    -1 & 0  \\
  \end{bmatrix}
   \quad   \bf{k} =
  \begin{bmatrix}
    0 & i \\
    i & 0  \\
  \end{bmatrix}
\end{equation}

\begin{definition}
  Let $\HH$ be the set of all matrices of the form
  \begin{equation}
    \label{eq:128}
    p_0 e + p_1 {\bf i} + p_2 {\bf j} + p_3 {\bf k}
  \end{equation}
  where $(p_0,p_1,p_2,p_3) \in \RR^4$. Every Matrix in $\HH$ is of the form
  \begin{equation}
    \label{eq:129}
    \begin{bmatrix}
      x &y  \\
      - \bar y  & \bar x
    \end{bmatrix}
  \end{equation}
where $x = p_0 + i p_1$ and $y = p_2 + i p_3$. The matrices in $\HH$ are called quaternions.
\end{definition}


\begin{definition}
  The null quaternion generated by $[0,0,0,0] \in \RR^4$ is denoted by $0$ . Quaternions of the form $p_1 \bf {i} + p_2 \bf{j} + p_3 \bf{k}$ are called pure quaternions. The set of pure quaternions is denoted by $\HH_p$.
\end{definition}

With the definition of $\HH$ as a set of complex matrices, It can be show that $\HH$ is a real vector space of dimension $4$ with basis ${e, \bf, i, j, k}$. Furthermore, with the matrix product, $\HH$ is a real algebra.

\paragraph{Representation of quaternions} Thanks to the equations~\eqref{eq:128}, ~\eqref{eq:128} and ~\eqref{eq:129}, we see that there are several manner to represent a quaternion $p\in \HH$. It can be represented as a complex matrix as in~\eqref{eq:129}. It can also be represented as a vector in $\RR^4$ , $p= [p_0,p_1,p_2,p_3]$ with the isomorphism~\eqref{eq:128}. In other words,  $\HH$ is isomorphic to $\RR^4$.  The first element $p_0$ can also be viewed as  a scalar and three last ones as a vector in $\RR^3$ denoted by $\vv{p} = [p_1,p_2,p_3]$, and in that case, $\HH$ is viewed as $\RR\times \RR^3$. The quaternion can be written as $p=(p_0,\vv{p})$.

\paragraph{Quaternion product} The quaternion product denoted by $p \glaw q $ for $p,q\in \HH_1$ is naturally defined as the product of complex matrices. With its representation in $\RR\times \RR^3$, the quaternion product is defined by
\begin{equation}
  \label{eq:73}
  p \glaw q =
  \begin{bmatrix}
    p_oq_o - \vv{p}\vv{q} \\
    p_0\vv{q}+q_o\vv{p} + \vv{p}\times\vv{q}
  \end{bmatrix}.
\end{equation}
Since the product is a matrix product, it is not communicative, but it is associative.  The identity element for the quaternion product is
\begin{equation}
e=  \begin{bmatrix}
    1 & 0 \\
    0 & 1  \\
  \end{bmatrix} =(1,\vv{0})\label{eq:57}.
\end{equation}Let us note that
\begin{equation}
  \label{eq:74}
  (0,\vv{p})\glaw (0,\vv(q)) = - (0,\vv{q})\glaw (0,\vv{p}).
\end{equation}
The quaternion multiplication can also be represented as a matrix operation in $\RR^{4\times4}$. Indeed, we have
\begin{equation}
  \label{eq:75}
  p \glaw q  =
  \begin{bmatrix}
    q_0 p_0 -q_1p_1-q_2p_2-q_3p_3\\
    q_0 p_1 +q_1p_0-q_2p_3+q_3p_2\\
    q_0 p_2 +q_1p_3+q_2p_0-q_3p_1\\
    q_0 p_3 -q_1p_2+q_2p_1+q_3p_0\\
  \end{bmatrix}
\end{equation}
that can be represented as
\begin{equation}
  \label{eq:76}
  p \glaw q  =
  \begin{bmatrix}
    p_0 & -p_1 & -p_2 & -p_3 \\
    p_1 & p_0 & -p_3 & p_2 \\
    p_2 & p_3 & p_0 & -p_1 \\
    p_3 & -p_2 & p_1 & p_0 \\
  \end{bmatrix}
  \begin{bmatrix}
    q_0\\
    q_1\\
    q_2\\
    q_3
  \end{bmatrix} := [p_\glaw]q
\end{equation}
or
\begin{equation}
  \label{eq:77}
  p \glaw q  =
  \begin{bmatrix}
    q_0 & -q_1 & -q_2 & -q_3 \\
    q_1 & q_0 & q_3 & -q_2 \\
    q_2 & -q_3 & q_0 & q_1 \\
    q_3 & q_2 & -q_1 & q_0 \\
  \end{bmatrix}
  \begin{bmatrix}
    p_0\\
    p_1\\
    p_2\\
    p_3
  \end{bmatrix} := [{}_\glaw q] p
\end{equation}

\paragraph{Adjoint quaternion, inverse and norm}
The adjoint quaternion of $p$ is denoted by
\begin{equation}
  p^\star = \overline{  \begin{bmatrix}
      x &y  \\
      - \bar y  & \bar x
    \end{bmatrix}}^T
  =\begin{bmatrix}
    \bar x & - y  \\
    \bar y  &  x
  \end{bmatrix} =
  \begin{bmatrix}
    p_0, -p_1, -p_2, -p_3
  \end{bmatrix} = (p_0, - \vv{p})
\end{equation}
We note that
\begin{equation}
  \label{eq:131}
  p \glaw p^\star =
  \begin{bmatrix}
      x &y  \\
      - \bar y  & \bar x
    \end{bmatrix}
    \begin{bmatrix}
    \bar x & - y  \\
    \bar y  &  x
  \end{bmatrix} = \det(\begin{bmatrix}
      x &y  \\
      - \bar y  & \bar x
    \end{bmatrix}) e = (x\bar x + y \bar y) e  = (p^2_0 + p^2_1+ p_2^2 + p_3^2)e
\end{equation}


The norm of a quaternion is given by $|p|^2=p^\top p = p_o^2+p_1^2+p_2^2+p_3^2$. In particular, we have $p \glaw p^\star = p^\star \glaw p = |p|^2 e$. This allows to define the reciprocal of a non zero quaternion by
\begin{equation}
  \label{eq:78}
  p ^{-1} = \frac 1 {|p|^2} p^\star
\end{equation}
A quaternion $p$ is said to be unit if $|p| =1$.

\paragraph{Unit quaternion and rotation}
For two vectors $x\in \RR^3$ and $x'\in \RR^3$, we define the quaternion $p_x = (0,x)\in \HH_p$ and  $p_{x'} = (0,x')\in \HH_p$.
For a given unit quaternion $p$, the transformation
\begin{equation}
  \label{eq:79}
  p_{x'} = p \glaw p_x \glaw  p^\star
\end{equation}
defines a rotation $R$ such that $x'  = R x$ given by
\begin{equation}
  \label{eq:80}
  x' = (p_0^2- p^\top \vv{p}) x +2 p_0(\vv{p}\times x) +  2 (\vv{p}^\top x) p = R x
\end{equation}
The rotation matrix may be computed as
\begin{equation}
  \label{eq:81}
  R = \Phi(p) =
  \begin{bmatrix}
    1-2 p_2^2- 2 p_3^2 & 2(p_1p_2-p_3p_0) & 2(p_1p_3+p_2p_0)\\
    2(p_1p_2+p_3p_0) & 1-2 p_1^2- 2 p_3^2 & 2(p_2p_3-p_1p_0)\\
    2(p_1p_3-p_2p_0) & 2(p_2p_3+p_1p_0)  & 1-2 p_1^2- 2 p_2^2\\
  \end{bmatrix}
\end{equation}


\paragraph{Computation of the time derivative of a unit  quaternion associated with a rotation.}
The derivation with respect to time can obtained as follows. The rotation transformation for a unit quaternion is given by
\begin{equation}
  \label{eq:82}
  p_{x'}(t) = p(t) \glaw p_x \glaw p^\star(t) =  p(t) \glaw p_x \glaw p^{-1}(t)
\end{equation}
and can be derived as
\begin{equation}
  \label{eq:83}
  \begin{array}{lcl}
    \dot p_{x'}(t) &=& \dot p(t) \glaw p_x \glaw p^{-1}(t) + p(t) \glaw p_x \glaw \dot p^{-1}(t) \\
                  &=& \dot p(t) \glaw p^{-1}(t)  \glaw   p_{x'}(t)  +      p_{x'}(t) \glaw p(t)  \glaw \dot p^{-1}(t)
  \end{array}
\end{equation}
From $p(t) \glaw p^{-1}(t) =e$, we get
\begin{equation}
  \label{eq:84}
  \dot p(t) \glaw p^{-1}(t) + p \glaw \dot p^{-1}(t) = 0
\end{equation}
so (\ref{eq:82}) can be rewritten
\begin{equation}
  \label{eq:85}
  \begin{array}{lcl}
    \dot p_{x'}(t) = \dot p(t) \glaw p^{-1}(t)   \glaw   p_{x'}(t)  -    p_{x'}(t) \glaw  \dot p(t) \glaw p^{-1}(t)
  \end{array}
\end{equation}
The scalar part of $\dot p(t) \glaw p^{-1}(t)$ is $(\dot p(t) \glaw p^{-1}(t))_0 = p_o \dot p_0 + \vv{p}^T\vv{\dot p}$. Since $p$ is a unit quaternion, we have
\begin{equation}
  \label{eq:86}
  |p|=1 \implies \frac{d}{dt} (p^\top p) = 0 =  \dot p^\top p + p^\top \dot p =   2( p_o \dot p_0 + \vv{p}^T\vv{\dot p}).
\end{equation}
Therefore, the scalar part $(\dot p(t) \glaw p^{-1}(t))_0 =0$.
The quaternion product $\dot p(t) \glaw p^{-1}(t)$ and  $p_{x'}(t)$ is a product of quaternions with zero scalar part (see~\eqref{eq:74}), so we have
\begin{equation}
  \label{eq:87}
  \begin{array}{lcl}
    \dot p_{x'}(t) = 2 \dot p(t) \glaw p^{-1}(t)   \glaw p_{x'}(t).
  \end{array}
\end{equation}
In terms of vector of $\RR^3$, this corresponds to
\begin{equation}
  \label{eq:88}
  \dot x'(t) = 2 \vv{ \dot p(t) \glaw p^{-1}(t) } \times x'(t).
\end{equation}
Since $x'(t) = R(t) x$, we have $\dot x' = \dot R(t) x = \tilde \omega(t) R(t) x  = \tilde \omega(t) x'(t) $. Comparing \eqref{eq:87} and \eqref{eq:88}, we get
\begin{equation}
  \label{eq:89}
  \tilde \omega(t)  = 2 \vv{ \dot p(t) \glaw p^{-1}(t) }
\end{equation}
or equivalently
\begin{equation}
  \dot p(t) \glaw p^{-1}(t) = (0, \frac{\omega(t)}{2} )
  \label{eq:90}
\end{equation}
Finally, we can conclude that
\begin{equation}
  \label{eq:91}
  \dot p(t) = (0, \frac{\omega(t)}2 ) \glaw p(t).
\end{equation}
Since $\omega(t)=R(t)\Omega(t)$, we have
\begin{equation}
  \label{eq:92}
  (0, \omega(t) ) = (0, R(t) \Omega(t) ) = p(t) \glaw (0, \Omega(t) ) \glaw \bar p(t) = p(t) \glaw (0, \Omega(t) ) \glaw  p^{-1}(t)
\end{equation}
and then
\begin{equation}
  \label{eq:93}
  \dot p(t) =\frac 1 2 p(t) \glaw(0, \Omega(t) ) .
\end{equation}

The time derivation is compactly written
\begin{equation}
  \label{eq:94}
  \dot p = \frac  1 2 p  \glaw(0, \frac\Omega 2 ) =  [p_\glaw] p_{\frac \Omega 2} = \Psi(p)\frac \Omega 2,
\end{equation}
and using the matrix representation of product of  quaternion
we get
\begin{equation}
  \label{eq:95}
  \Psi(p) =  \begin{bmatrix}
    -p_1 & -p_2 & -p_3 \\
    p_0 & -p_3 & p_2 \\
    p_3 & p_0 & -p_1 \\
    -p_2 & p_1 & p_0 \\
  \end{bmatrix}
\end{equation}
The relation \eqref{eq:93} can be also inverted by writing
\begin{equation}
  \label{eq:96}
   (0, \Omega(t) ) = 2 p^{-1}(t) \glaw \dot p(t)
\end{equation}
Using again  matrix representation of product of  quaternion, we get
\begin{equation}
  \label{eq:97}
  \Omega(t)  = 2 \vv{p^{-1}(t) \glaw \dot p(t)}  = 2  \begin{bmatrix}
    -p_1 & p_0 & p_3 & -p_2 \\
    -p_2 & -p_3 & p_0 & p_1 \\
    -p_3 & p_2 & -p_1  & p_0\\
  \end{bmatrix}\dot p(t) = 2 \Psi(p)^\top \dot p(t)
\end{equation}
Note that we have $\Psi^\top(p)\Psi(p)= I_{4\times 4 }$ and  $\Psi(p)\Psi^\top(p)= I_{3\times 3 }$

\paragraph{Lie group structure of unit quaternions.} In terms of complex matrices, an unit quaternion $p$ satisfies
\begin{equation}
  \label{eq:125}
  \det\left(    \begin{bmatrix}
      x &y  \\
      - \bar y  & \bar x
    \end{bmatrix} \right) =1
\end{equation}
The set of all unit quaternions that we denote $\HH_1$ is the set of unitary matrices of determinant equal to $1$. From~\eqref{eq:131}, we get that
\begin{equation}
  \label{eq:126}
  p \glaw p^\star = e
\end{equation}
It implies that the set $\HH_1$ is the set of special unitary complex matrices. The set is a Lie group usually denoted as $SU(2)$. Since we used multiple representation of a quaternion, we continue to use $\HH_1 \cong SU(2)$ as a notation but with the Lie group structure implied by $SU(2)$.

Let us compute the tangent vector at a point $p \in \HH_1$. Let $q(t)$ be  a smooth curve $q(\cdot) : t\in \RR \mapsto q(t)\in H_1$ in $H_1$ such that $q(O)= p$.
Since $q(t)\in H_1$, we have $|q(t)|=1$ and then $\frac{d}{dt} |q(t)| = 2(q_0(0) \dot q_0(0) + \vec{q}^T(0) \vec{\dot q}(0) ) =0$. At $t=0$, we get
\begin{equation}
  \label{eq:48}
  2(p_0 a_0 + \vec{p}^T \vec{a})= 0.
\end{equation}
This relation imposes that the quaternions $2 p^\star \glaw a \in H_1$ and $2 a \glaw p^\star \in H_1$, that is, have to be pure quaternions. Therefore, it exists $\omega \in \RR^3$  and $\Omega \in \RR^3$ such that
\begin{equation}
  \label{eq:132}
  (0, \Omega) = 2 p ^\star \glaw a
\end{equation}
and
\begin{equation}
  \label{eq:132}
  (0, \omega) = 2 a\glaw p^\star
\end{equation}
In other terms, the tangent vector spaces at $p \in \HH_1$ can be represented as a left representation
\begin{equation}
  \label{eq:133}
  T_p\HH_1 = \{ a \mid a = p \glaw (0, \frac \Omega 2 ), \Omega \in \RR^3\}
\end{equation}
or a right representation
\begin{equation}
  \label{eq:1330}
  T_p{\HH_1} = \{ a \mid a =  (0, \frac \omega 2 ) \glaw p, \omega \in \RR^3\}
\end{equation}

At $p=e$, we get the Lie algebra defined by
\begin{equation}
  \label{eq:134}
  \mathfrak h_1 =  T_e{\HH_1} =  \{ a = (0, \frac \Omega 2 ), \Omega \in \RR^3 \}
\end{equation}
equipped with the Lie bracket given by the commutator
\begin{equation}
  \label{eq:135}
  [p,q] = p \glaw q- q\glaw p.
\end{equation}
We can easily verify that for $a = (0, \frac \Omega 2 ), \, b = (0, \frac \Gamma 2 ) \in \mathfrak h_1 $, we have
\begin{equation}
  \label{eq:136}
  [a,b] = (0, \frac \Omega 2 ) \glaw (0, \frac \Gamma 2 ) - (0, \frac \Gamma 2 ) \glaw  (0, \frac \Omega 2 )  = (0, \frac{\Omega\times \Gamma}{2}) \in \mathfrak h_1
\end{equation}
As for $\mathfrak so(3)$,  the Lie algebra $\mathfrak h_1$ is isomorphic to $\RR^3$ thanks to the operator $\widehat{(\cdot)} :\RR^3 \rightarrow \mathfrak h_1$ and defined by
\begin{equation}
  \label{eq:54}
 \widehat{(\cdot)}: \Omega \mapsto \widehat \Omega = (0, \frac \Omega 2 )
\end{equation}
With this operator, the Lie Bracket can be written
\begin{equation}
  \label{eq:137}
  [\widehat{\Omega},\widehat{\Gamma}] = \widehat{\Omega \times \Gamma}
\end{equation}


\paragraph{ A special  (right)  action of Lie Group $\mathcal G$ on a manifold $\mathcal M$. }
Let us come back to the representation of  $T_p\HH_1$ given in~\eqref{eq:133}. It is clear it can expressed with a representation that relies on $\mathfrak h_1$
\begin{equation}
  \label{eq:158}
   T_RSO(3) = \{ a = p \glaw  \widehat \Omega \mid \widehat \Omega \in \mathfrak h_1 \}.
\end{equation}
With \eqref{eq:58}, we see that there is a linear map that relates $T_p\HH_1$ to  $\mathfrak h_1$. This linear map defines a vector field.
A special group action is defined by the left translation map for a point $p \in \HH_1$
\begin{equation}
  \label{eq:159}
  \begin{array}[lcl]{rcl}
    L_p& :&   \HH_1 \rightarrow  \HH_1\\
       & &  q  \mapsto L_p(q) = p \glaw q\\
  \end{array}
\end{equation}
which is diffeomorphism on $\HH_1$. In that case, we identify the manifold and the group. So, $L_p$ can be viewed as a left or a right group action. We choose a right action. For our application where $\mathcal G = \mathcal M = \HH_1$ and $\Lambda^r(p,q) = L_{p}(q) =  p \glaw q $, we get
\begin{equation}
  \label{eq:160}
   \lambda^r_{*}(a)(p) = \left. \frac{d}{dt} L_{p}(q(t)) \right|_{t=0}  = \left. \frac{d}{dt} p \glaw q(t) \right|_{t=0} =  p \glaw \dot q(0) = p  \glaw \dot q(0)  \in X(\mathcal M)
 \end{equation}
 for a smooth curve $q(t)$ in $\HH_1$.
Since $q(\cdot)$ is a smooth curve in $\HH_1$, $\dot q(0)$ is a tangent vector at the point $q(0)=I$, that is an element $a = \widehat  \Omega  \in \mathfrak h_1 $ defined by the relation~\eqref{eq:33}. Therefore, the vector field in \eqref{eq:160} is a tangent vector field and we get
\begin{equation}
  \label{eq:161}
  \dot p(t) = \lambda^r_{*}(a)(p(t)) = p(t)  \glaw \widehat \Omega
\end{equation}

\paragraph{Exponential map $\expq : \mathfrak h_1 \rightarrow \HH_1$}
We can directly apply Theorem~\ref{Theorem:solutionofLieODE} and we get that the solution of
\begin{equation}
  \label{eq:130}
  \begin{cases}
  \dot p(t) = \lambda^r_{*}(a)(p(t)) = p(t) \glaw \widehat \Omega \\
  p(0) = Rp_0
\end{cases}
\end{equation}
 is
\begin{equation}
  \label{eq:138}
  p(t) = p_0 \expq(t \widehat \Omega)
\end{equation}
The exponential mapping $\expq : \mathfrak h_1 \rightarrow \HH_1$ can also be defined as $\expq(\widehat \Omega) = q(1)$ where $q (t)$ satisfies the  differential equation
\begin{equation}
  \label{eq:235}
  \dot q(t) = q(t) \cdot \widehat \Omega , \quad q (0) = e.
\end{equation}
Using the quaternion product, the exponential map can be expressed as
\begin{equation}
  \label{eq:232}
  \expq(t \widehat \Omega ) = \sum_{k=0}^\infty \frac{(t\widehat \Omega)^k}{k!}
\end{equation}
since it is  a solution of \eqref{eq:130}. A simple computation allows to check this claim:
\begin{equation}
  \label{eq:233}
   \frac{d}{dt}\expq(t \widehat \Omega ) = \sum_{k=1}^\infty  k t^{k-1} \frac{ \widehat \Omega ^k}{k!} =  \sum_{k=0}^\infty  t^{k} \frac{t \widehat \Omega ^k}{k!}\glaw  \widehat \Omega  =   \expq(t \widehat \Omega ) \glaw \widehat \Omega.
\end{equation}

A closed form relation for the form the quaternion exponential can also be found by noting that
\begin{equation}
  \label{eq:140}
  \widehat \Omega ^2  = - \left(\frac \theta 2 \right)^2 e, \text{ and } \widehat \Omega ^3  = - \left(\frac \theta 2 \right)^2 \widehat \Omega.
\end{equation}
A simple expansion of \eqref{eq:232} at $t=1$ equals
\begin{equation}
  \label{eq:141}
  \begin{array}{lcl}
    \expq(\widehat \Omega ) &=& \sum_{k=0}^\infty \frac{(\widehat \Omega)^k}{k!}\\
                            &=& \sum_{k=0}^\infty \frac{(-1)^k}{(2k)!}\left(\frac \theta 2 \right)^{2k} e + \sum_{k=0}^\infty \frac{(-1)^k}{(2k+1)!} \left(\frac \theta 2 \right)^{2k+1} \widehat \Omega \\
                            &=& \cos(\frac \theta 2) e + \frac{\sin(\frac \theta 2)}{\frac \theta 2} \widehat \Omega \\
  \end{array}
\end{equation}
that is
\begin{equation}
  \label{eq:144}
  \expq(\widehat \Omega )  = (\cos(\frac \theta 2), \sin(\frac \theta 2) \frac{\Omega}{\theta}   ).
\end{equation}

\paragraph{Adjoint representation}
In the case of $\HH_1$, the definition of the operator $\Ad$ gives
\begin{equation}
  \label{eq:121}
  \Ad_p(\widehat\Omega)  = p\glaw \widehat\Omega p^\star
\end{equation}
 and then mapping $\ad_{\widehat\Omega}(\widehat \Gamma)$ is defined by
\begin{equation}
  \label{eq:56}
  \ad_{\widehat\Omega}(\widehat\Gamma) = \widehat \Omega \widehat\Gamma - \widehat \Gamma \widehat\Omega  =  [\widehat \Omega,\widehat \Gamma] = \widehat{\Omega \times \Gamma}.
\end{equation}
Using the isomorphism between $\mathfrak h_1$ and $\RR^3$, we can use the  the mapping $\ad_{\Omega}(\Gamma) : \RR^3\times\RR^3 \rightarrow \RR^3$ given by \eqref{eq:55} to get
\begin{equation}
  \label{eq:145}
   \ad_{\widehat\Omega}(\widehat\Gamma) = \widehat{\Omega \times \Gamma} = \widehat{\ad_{\Omega}(\Gamma)} =  \widehat{\tilde \Omega \Gamma}
\end{equation}

\paragraph{Differential of the exponential map $\dexpq$}
The differential of the exponential mapping, denoted by $\dexpq$ is defined as the 'right trivialized' tangent of the exponential map
\begin{equation}
  \label{eq:71}
  \frac{d}{dt} (\expq(\widehat \Omega(t))) = \dexpq_{\widehat\Omega(t)}(\frac{d \widehat{\Omega}(t)}{dt}) \expq(\widehat\Omega(t))
\end{equation}

An explicit expression of $\dexp_{\widehat\Omega}(\widehat\Gamma)$ can also be developed either by developing the expansion and~\eqref{eq:137}.
\begin{equation}
  \label{eq:168}
   \dexpq_{\widehat\Omega}(\Gamma) = \sum_{k=0}^\infty \frac{1}{(k+1)!} \ad_{\widehat\Omega}^k (\widehat\Gamma) = \widehat{T(\Omega)\Gamma}
\end{equation}

\begin{remark}
Note that the time derivative in $\RR^4$ is not differential mapping.
The standard time derivative of $\expq$ in the expression \eqref{eq:144} gives
\begin{equation}
  \label{eq:171}
    \frac{d}{dt}\expq(\widehat \Gamma(t)) = (- \frac{\sin(\theta)}{\theta} \Omega^T\Gamma, \frac{\sin(\theta)}{\theta}\Gamma  +\frac{\theta \cos(\theta)-\sin(\theta)}{\theta^3}\Omega^T\Omega \Gamma  )
\end{equation}
that can be expressed in $\RR^4$ by
\begin{equation}
  \label{eq:175}
  \frac{d}{dt}\expq(\widehat \Gamma(t))  = \nabla \expq(\widehat\Omega) \widehat{\dot\Omega}
\end{equation}
with
\begin{equation}
  \label{eq:176}
  \nabla \expq(\widehat\Omega) =
  \begin{bmatrix}
    - \frac{\sin(\theta)}{\theta} \Omega^T \\
    \frac{\sin(\theta)}{\theta}I  +\frac{\theta \cos(\theta)-\sin(\theta)}{\theta^3}\Omega^T\Omega
  \end{bmatrix}
\end{equation}

Clearly, we have
\begin{equation}
  \label{eq:177}
  \nabla \expq(\widehat\Omega) \neq  \dexpq_{\widehat\Omega}
\end{equation}
\end{remark}


\paragraph{Directional derivative and Jacobians of functions of a quaternion}
\begin{ndrva}
  experimental
\end{ndrva}

Let $f : \HH_1 \rightarrow \RR $ be a mapping from the group to $\RR^3$. The directional derivative of $f$ in the direction $\widehat \Omega \in \mathfrak h_1$ at $p\in \HH_1$ is
defined by
\begin{equation}
  \label{eq:139}
 df_p(\widehat \Omega) =\left. \frac{d}{dt} f(p\glaw \expq(t\widehat \Omega)) \right|_{t=0}
\end{equation}

As a first simple example let us choose $f(p) = \vv{p \glaw p_x \glaw p^\star}$ for a given $x \in \RR^3 $, we get
\begin{equation}
  \label{eq:142}
  \begin{array}{lcl}
    D Id \cdot \widehat \Omega (p) = (\widehat \Omega^r f )(p) &=& \left. \frac{d}{dt}\vv{p\glaw \expq(t\widehat \Omega) \glaw p_x \glaw (p \glaw \expq(t\widehat \Omega))^\star}  \right|_{t=0}\\
                                                               & = & \vv{p\glaw \frac{d}{dt}\left. \expq(t\widehat \Omega) \right|_{t=0} \glaw p_x \glaw p^\star +  p \glaw p_x \glaw (p \glaw\frac{d}{dt}\left. \expq(t\widehat \Omega) \right|_{t=0})^\star}\\
  \end{array}
\end{equation}
We have form the definition of the time derivative of the exponential
\begin{equation}
  \label{eq:143}
  \begin{array}{lcl}
    \frac{d}{dt}\left. \expq(t\widehat \Omega) \right|_{t=0} &=&  \left. \dexpq_{\widehat\Omega}(\widehat \Omega)\expq(t\widehat \Omega) \right|_{t=0} \\
                                                            &=&  \dexpq_{\widehat\Omega}(\widehat \Omega)
  \end{array}
\end{equation}

Then, the directional derivative can be written
\begin{equation}
  \label{eq:146}
  \begin{array}{lcl}
    D Id \cdot \widehat \Omega (p) &=& \vv{p\glaw \dexpq_{\widehat\Omega}(\widehat \Omega)\glaw p_x \glaw p^\star  + p \glaw p_x \glaw (\dexpq_{\widehat\Omega}(\widehat \Omega))^* \glaw  p^\star } \\
  &=& \vv{p\glaw ( \dexpq_{\widehat\Omega}(\widehat \Omega)\glaw p_x +   p_x \glaw (\dexpq_{\widehat\Omega}(\widehat \Omega))^*) \glaw  p^\star }
  \end{array}
\end{equation}


\section{Newton-Euler equation in quaternion  form}

\paragraph{Computation of $T$ for unit quaternion} The operator $T(q)$ is directly obtained as
\begin{equation}
  T(q)=\frac 1 2 \label{eq:98}
  \begin{bmatrix}
    2 I_{3\times 3} & & 0_{3\times 3} & \\
    &   -p_1 & -p_2 & -p_3 \\
    0_{4\times 3}  &  p_0 & -p_3 & p_2 \\
    & p_3 & p_0 & -p_1 \\
    & -p_2 & p_1 & p_0
  \end{bmatrix}
\end{equation}

\paragraph{}


\begin{ndrva}
  todo :
  \begin{itemize}
  \item computation of the directional derivative of $R(\Omega)= exp(\tilde \Omega)$ in the direction $\tilde\Omega$, to get $T(\Omega)$
  \end{itemize}
\end{ndrva}

\paragraph{Quaternion representation}If the Lie group is described by unit quaternion, we get
\begin{equation}
  \label{eq:99}
  SO(3) = \{p = (p_0,\vv{p}) \in \RR^{4}\mid |p|=1  \}
\end{equation}
with the composition law  $p_1\glaw p_2$ given by the quaternion product.


Note that the concept of exponential map for Lie group that are not parameterized by matrices is also possible.


\subsection{Mechanical systems  with bilateral and unilateral constraints}
\label{section22}


Let us consider that the system~(\ref{eq:Newton-Euler-compact}) is  subjected to $m$ constraints, with $m_{e}$ holonomic bilateral
constraints
\begin{equation}
  \label{eq:bilateral-constraints}
  h^\alpha(q)=0, \alpha \in \mathcal{E}\subset\NN,  |\mathcal E| = m_e,
\end{equation}
and  $m_{i}$ unilateral constraints
\begin{equation}
  \label{eq:unilateral-constraints}
  g_{\n}^\alpha(q)\geq 0, \alpha \in \mathcal{I}\subset\NN,  |\mathcal I| = m_i.
\end{equation}
%
Let us denote as $J^\alpha_h(q) = \nabla^\top_q h^\alpha(q)  $ the Jacobian matrix of the bilateral constraint $h^\alpha(q)$ with respect to $q$ and as $J^\alpha_{g_\n}(q)$ respectively for $g_{\n}^\alpha(q)$  .
%
The bilateral constraints at the velocity level can be obtained as:
\begin{equation}
  \label{eq:bilateral-constraints-velocity}
 0 = \dot h^\alpha(q)= J^\alpha_h(q)\dot q = J^\alpha_h(q) T(q) v \coloneqq H^\alpha(q)  v,\quad  \alpha \in \mathcal{E}.
\end{equation}
By duality and introducing a Lagrange multiplier $\lambda^\alpha, \alpha \in \mathcal E$, the constraint generates a force applied to the body equal to $H^{\alpha,\top}(q)\lambda^\alpha$. For the unilateral constraints, a Lagrange multiplier $\lambda_{\n}^\alpha, \alpha \in \mathcal I$ is also associated and the constraints at the velocity level can also be derived as
\begin{equation}
  \label{eq:unilateral-constraints-velocity}
 0 \leq  \dot g_\n^\alpha(q)= J^\alpha_{g_\n}(q) \dot q = J^\alpha_{g_\n}(q)  T(q) v , \text{ if } g_{\n}^\alpha(q) = 0,\quad  \alpha \in \mathcal{I}.
\end{equation}
Again, the force applied to the body is given by $(J^\alpha_{g_\n}(q) T(q))^\top\lambda^\alpha_\n$. {Nevertheless, there is no reason that $\lambda^\alpha_\n =r^\alpha_\n$ and $u_\n = J^\alpha_{g_\n}(q) T(q) v$ if the $g_n$ is not chosen as the signed distance (the gap function)}. This is the reason why  we prefer  directly define the normal and the tangential local relative velocity with respect to the {twist vector} as
\begin{equation}
  \label{eq:unilateral-constraints-velocity-kinematic1}
   u^\alpha_\n  \coloneqq G_\n^\alpha(q) v, \quad u^\alpha_\t  \coloneqq G_\t^\alpha(q) v, \quad \alpha \in \mathcal{I},
\end{equation}
and the associated force as $G_\n^{\alpha,\top}(q) r^{\alpha}_\n $ and $G_\t^{\alpha,\top}(q) r^{\alpha}_\t$. For the sake of simplicity, we use the notation $u^\alpha  \coloneqq G^\alpha(q) v$ and its associated total force generated by the contact $\alpha$ as $G^{\alpha,\top}(q) r^{\alpha} \coloneqq G_\n^{\alpha,\top}(q) r^{\alpha}_\n + G_\t^{\alpha,\top}(q) r^{\alpha}_\t $.

The complete system of equation of motion can finally be written as
\begin{numcases}{ }
  ~~\dot q = T(q)v ,\nonumber \\[0.5ex]
  ~~ M \dot v  = F(t,q,v) + H^\top(q) \lambda +  G^\top(q) r, \nonumber \\ [0.5ex]
  ~~\begin{array}{ll}
    H^\alpha(q) v  =  0 ,& \alpha \in \mathcal E \\[1ex]
    \left. \begin{array}{ll}
      r^\alpha= 0 , &\text{ if } g_{\n}^\alpha(q) > 0,\\[1ex]
      {K}^{\alpha,*} \ni \widehat u^\alpha  \bot~ r^\alpha \in {K}^\alpha, &\text{ if } g_{\n}^\alpha(q) = 0, \\[1ex]
      u_{\n}^{\alpha,+} = -e_r^\alpha u_{\n}^{\alpha,-}, &\text{ if } g_{\n}^\alpha(q) = 0 \text{ and } u_{\n}^{\alpha,-} \leq 0,
    \end{array}\right\} & \alpha \in \mathcal I  \label{eq:NewtonEuler-uni}
\end{array}
\end{numcases}
where the definition of the variables $\lambda\in \RR^{m_e}, r\in \RR^{3m_i}$ and the operators $H,G$ are extended to collect all the variables for each constraints.

Note that all the constraints are written at the velocity integrators. {Another strong advantage is the straightforward introduction of  the contact dissipation processes that are naturally written at the velocity level such as the Newton impact law and the Coulomb friction. Indeed, in Mechanics, dissipation processes are always given in terms of rates of changes, or if we prefer, in terms of velocities.}

\paragraph{Siconos Notation} In the siconos notation, we have for the applied torques on the system the following decomposition
\begin{equation}
  F(t,q,v):= \begin{pmatrix}
    f(t,x_{\cg},  v_{\cg}, R, \Omega ) \\
    I \Omega \times \Omega + M(t,x_{\cg}, v_{\cg}, R, \Omega )
  \end{pmatrix}
  := \begin{pmatrix}
    f_{ext}(t)  - f_{int}(x_{\cg},  v_{\cg}, R, \Omega ) \\
    - M_{gyr}(\Omega) + M_{ext}(t) -  M_{int}(x_{\cg}, v_{\cg}, R, \Omega )
  \end{pmatrix}.
\end{equation}
with
\begin{equation}
  M_{gyr} := \begin{pmatrix}
     \Omega \times I\Omega
  \end{pmatrix}
\end{equation}


In the siconos notation, we have for the relation
\begin{equation}
  \label{eq:100}
   C =   J^\alpha(q) \quad CT = J^\alpha(q)T(q)
\end{equation}


\section{Time integration scheme in scheme}


\subsection{Moreau--Jean scheme based on a  $\theta$-method}
The complete Moreau--Jean scheme based on a  $\theta$-method is written as follows
 \begin{equation}
    \label{eq:Moreau--Jean-theta}
    \begin{cases}
      ~~\begin{array}{l}
        q_{k+1} = q_{k} + h T(q_{k+\theta}) v_{k+\theta} \quad \\[1ex]
        M(v_{k+1}-v_k) - h  F(t_{k+\theta}, q_{k+\theta},v_{k+\theta}) =  H^\top(q_{k+1}) Q_{k+1} + G^\top(q_{k+1}) P_{k+1},\quad\,\\[1ex]
      \end{array}\\
      ~~\begin{array}{lcl}
        \begin{array}{l}
          H^\alpha(q_{k+1}) v_{k+1}  =  0\\
        \end{array} & \left. \begin{array}{l}
          \vphantom{H^\alpha(q_{k+1}) v_{k+1}  =  0}\\[1ex]
        \end{array}\right\}    &\alpha \in \mathcal E  \\[1ex]
      ~~~P_{k+1}^\alpha= 0, &
      \left. \begin{array}{l}
          \vphantom{P_{k+1}^\alpha= 0,  \delta^\alpha_{k+1}=0}\\[1ex]
        \end{array}\right\}   & \alpha \not\in \mathcal I^\nu \\[1ex]
      %
      %
      \begin{array}{l}
          {K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha \\
      \end{array} &
      \left.\begin{array}{l}
          \vphantom{{K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha} \\
        \end{array}\right\}
      &\alpha \in \mathcal I^\nu\\
  \end{array}
\end{cases}
\end{equation}
where $\mathcal I^\nu$ is the set of forecast constraints, that may be evaluated as
\begin{equation}
  \label{eq:101}
  \mathcal I^\nu = \{\alpha \mid \bar g_\n^\alpha \coloneqq g_\n + \frac h 2 u^\alpha_\n \leq 0\}.
\end{equation}


\subsection{Semi-explicit version Moreau--Jean scheme based on a  $\theta$-method}

\begin{equation}
    \label{eq:Moreau--Jean-explicit}
    \begin{cases}
      ~~\begin{array}{l}
        q_{k+1} = q_{k} + h T(q_{k}) v_{k+\theta} \quad \\[1ex]
        M(v_{k+1}-v_k) - h  F(t_{k}, q_{k},v_{k}) =  H^\top(q_{k}) Q_{k+1}+  G^\top(q_{k}) P_{k+1},\quad\,\\[1ex]
      \end{array}\\
      ~~\begin{array}{lcl}
        \begin{array}{l}
          H^\alpha(q_{k+1}) v_{k+1}  =  0\\
        \end{array} & \left. \begin{array}{l}
          \vphantom{H^\alpha(q_{k+1}) v_{k+1}  =  0}\\[1ex]
        \end{array}\right\}    &\alpha \in \mathcal E  \\[1ex]
      ~~P_{k+1}^\alpha= 0, &
      \left. \begin{array}{l}
          \vphantom{P_{k+1}^\alpha= 0,  \delta^\alpha_{k+1}=0}\\[1ex]
        \end{array}\right\}   & \alpha \not\in \mathcal I^\nu \\[1ex]
      %
      %
      \begin{array}{l}
          {K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha \\
      \end{array} &
      \left.\begin{array}{l}
          \vphantom{{K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha} \\
        \end{array}\right\}
      &\alpha \in \mathcal I^\nu\\
  \end{array}
\end{cases}
\end{equation}

In this version, the new velocity $v_{k+1}$ can be computed explicitly, assuming that the inverse of $M$ is easily written, as

\begin{equation}
  \label{eq:Moreau--Jean-theta--explicit-v}
  v_{k+1}   =  v_k + M^{-1} h  F(t_{k}, q_{k},v_{k}) +  M^{-1} (H^\top(q_{k}) Q_{k+1}+  G^\top(q_{k}) P_{k+1})
\end{equation}


\subsection{Nearly implicit version Moreau--Jean scheme based on a  $\theta$-method implemented in siconos}

A first simplification is made considering a given value of $q_{k+1}$ in $T()$, $H()$ and $G()$ denoted by $\bar q_k$. This limits the computation of the Jacobians of this operators with respect to $q$.
\begin{equation}
    \label{eq:Moreau--Jean-theta-nearly}
    \begin{cases}
      ~~\begin{array}{l}
        q_{k+1} = q_{k} + h T(\bar q_k) v_{k+\theta} \quad \\[1ex]
        M(v_{k+1}-v_k) - h  \theta F(t_{k+1}, q_{k+1},v_{k+1}) - h (1- \theta) F(t_{k}, q_{k},v_{k})  =  H^\top(\bar q_k) Q_{k+1} + G^\top(\bar q_k) P_{k+1},\quad\,\\[1ex]
      \end{array}\\
      ~~\begin{array}{lcl}
        \begin{array}{l}
          H^\alpha(\bar q_k) v_{k+1}  =  0\\
        \end{array} & \left. \begin{array}{l}
          \vphantom{H^\alpha(q_{k+1}) v_{k+1}  =  0}\\[1ex]
        \end{array}\right\}    &\alpha \in \mathcal E  \\[1ex]
      ~~P_{k+1}^\alpha= 0, &
      \left. \begin{array}{l}
          \vphantom{P_{k+1}^\alpha= 0,  \delta^\alpha_{k+1}=0}\\[1ex]
        \end{array}\right\}   & \alpha \not\in \mathcal I^\nu \\[1ex]
      %
      %
      \begin{array}{l}
          {K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha \\
      \end{array} &
      \left.\begin{array}{l}
          \vphantom{{K}^{\alpha,*} \ni \widehat u_{k+1}^\alpha~ \bot~ P_{k+1}^\alpha \in {K}^\alpha} \\
        \end{array}\right\}
      &\alpha \in \mathcal I^\nu\\
  \end{array}
\end{cases}
\end{equation}
The nonlinear residu is defined as
\begin{equation}
  \label{eq:Moreau--Jean-theta--nearly-residu}
  \mathcal R(v) =  M(v-v_k) - h  \theta F(t_{k+1}, q(v),v) - h (1- \theta) F(t_{k}, q_{k},v_{k}) - H^\top(\bar q_k) Q_{k+1} - G^\top(\bar q_k) P_{k+1}
\end{equation}
with
\begin{equation}
  \label{eq:Moreau--Jean-theta--nearly-residu1}
  q(v) = q_{k} + h T(\bar q_k)) ((1-\theta) v_k + \theta v).
\end{equation}
At each time step, we have to solve
\begin{equation}
  \label{eq:Moreau--Jean-theta--nearly-residu2}
  \mathcal R(v_{k+1}) =  0
\end{equation}
together with the constraints.

Let us write a linearization of the problem to design a Newton procedure:
\begin{equation}
  \label{eq:Moreau--Jean-theta--nearly-residu3}
  \nabla^\top_v \mathcal R(v^{\tau}_{k+1})(v^{\tau+1}_{k+1}-v^{\tau}_{k+1}) = -  \mathcal R(v^{\tau}_{k+1}).
\end{equation}
The computation of $ \nabla^\top_v \mathcal R(v^{\tau}_{k+1})$ is as follows
\begin{equation}
  \label{eq:102}
  \nabla^\top_v \mathcal R(v) = M - h \theta \nabla_v F(t_{k+1}, q(v),v)
\end{equation}
with
\begin{equation}
  \label{eq:103}
  \begin{array}{lcl}
    \nabla_v F(t_{k+1}, q(v),v) &=& D_2 F(t_{k+1}, q(v),v) \nabla_v q(v) + D_3 F(t_{k+1}, q(v),v) \\
                                &=& h \theta D_2 F(t_{k+1}, q(v),v) T(\bar q_k) + D_3 F(t_{k+1}, q(v),v) \\
  \end{array}
\end{equation}
where $D_i$ denotes the derivation with respect the $i^{th}$ variable. The complete Jacobian is then given by
\begin{equation}
  \label{eq:104}
  \nabla^\top_v \mathcal R(v) = M - h \theta D_3 F(t_{k+1}, q(v),v) - h^2 \theta^2 D_2 F(t_{k+1}, q(v),v) T(\bar q_k)
\end{equation}
In siconos, we ask the user to provide the functions $D_3 F(t_{k+1}, q ,v )$ and $D_2 F(t_{k+1}, q,v)$.

Let us denote by $W^{\tau}$ the inverse of  Jacobian of the residu,
\begin{equation}
  \label{eq:105}
  W^{\tau} = (M - h \theta D_3 F(t_{k+1}, q(v),v) - h^2 \theta^2 D_2 F(t_{k+1}, q(v),v) T(\bar q_k))^{-1}.
\end{equation}
and by $\mathcal R_{free}(v)$ the free residu,
\begin{equation}
  \label{eq:106}
  \mathcal R_{free}(v) =  M(v-v_k) - h  \theta F(t_{k+1}, q(v),v) - h (1- \theta) F(t_{k}, q_{k},v_{k}).
\end{equation}

The linear equation \ref{eq:Moreau--Jean-theta--nearly-residu3} that we have to solve is equivalent to
\begin{equation}
  \label{eq:107}
  \boxed{v^{\tau+1}_{k+1} = v^{\tau}_{k+1} - W  \mathcal R_{free}(v^\tau_{k+1}) + W   H^\top(\bar q_k) Q^{\tau+1}_{k+1} + W G^\top(\bar q_k) P^{\tau+1}_{k+1}}
\end{equation}
We define  $v_{free}$ as
\begin{equation}
  \label{eq:108}
  v_{free}  = v^{\tau}_{k+1} - W  \mathcal R_{free}(v^\tau_{k+1})
\end{equation}

The local velocity at contact can be written
\begin{equation}
  \label{eq:109}
  u^{\tau+1}_{\n,k+1} = G(\bar q_k) [  v_{free}^{\tau} + W   H^\top(\bar q_k) Q^{\tau+1}_{k+1} + W G^\top(\bar q_k) P^{\tau+1}_{k+1}]
\end{equation}
and for the equality constraints
\begin{equation}
  \label{eq:110}
  u^{\tau+1}_{k+1} = H(\bar q_k) [  v_{free}^{\tau} + W   H^\top(\bar q_k) Q^{\tau+1}_{k+1} + W G^\top(\bar q_k) P^{\tau+1}_{k+1}]
\end{equation}
Finally, we get a linear relation between $u^{\tau+1}_{\n,k+1}$ and the multiplier
\begin{equation}
  \label{eq:111}
 \boxed{ u^{\tau+1}_{k+1} =
  \begin{bmatrix}
    H(\bar q_k) \\
    G(\bar q_k)
  \end{bmatrix} v_{free}^{\tau}
  +
  \begin{bmatrix}
    H(\bar q_k)W   H^\top(\bar q_k) & H(\bar q_k)W   G^\top(\bar q_k) \\
    G(\bar q_k)W   H^\top(\bar q_k) & G(\bar q_k)W   G^\top(\bar q_k) \\
  \end{bmatrix}
  \begin{bmatrix}
    Q^{\tau+1}_{k+1} \\
    P^{\tau+1}_{k+1}
  \end{bmatrix}}
\end{equation}


\paragraph{choices for $\bar q_k$} Two choices are possible for $\bar q_k$
\begin{enumerate}
\item $\bar q_k = q_k$
\item $\bar q_k = q^{\tau}_{k+1}$
\end{enumerate}

\begin{ndrva}

  todo list:

  \begin{itemize}


  \item add the projection step for the unit quaternion

  \item describe the computation of H and G that can be hybrid


\end{itemize}

\end{ndrva}


\subsection{Computation of the Jacobian in special case}

\paragraph{Moment of gyroscopic forces}
Let us denote by the basis vector $e_i$ given the $i^{th}$ column of the identity matrix $I_{3\times3}$. The Jacobian of $M_{gyr}$ is given by
\begin{equation}
  \label{eq:112}
  \nabla^\top_\Omega M_{gyr}(\Omega) = \nabla^\top_\Omega (\Omega \times I \Omega) =
  \begin{bmatrix}
    e_i \times I \Omega + \Omega \times I e_i, i =1,2,3
  \end{bmatrix}
\end{equation}

\paragraph{Linear internal wrench}
If the internal wrench  is given by
\begin{equation}
  \label{eq:113}
  F_{int}(t,q,v) =
  \begin{bmatrix}
    f_{int}(t,q,v)\\
    M_{int}(t,q,v)
  \end{bmatrix}
  = C v + K q, \quad C \in \RR^{6\times 6}, \quad K \in \RR^{6\times 7 }
\end{equation}
we get
\begin{equation}
  \label{eq:114}
  \begin{array}{lcl}
    \nabla_v F(t_{k+1}, q(v),v)  &=& h \theta K T(\bar q_k) + C \\
    \nabla^\top_v \mathcal R(v) &=& M - h \theta C - h^2 \theta^2 K T(\bar q_k)
  \end{array}
\end{equation}

\paragraph{External moment given in the inertial frame}

If the external moment denoted by $m_{ext} (t)$ is expressed in inertial frame, we have
\begin{equation}
  \label{eq:115}
  M_{ext}(q,t) = R^T m_{ext}(t)= \Phi(p) m_{ext}(t)
\end{equation}
In that case, $  M_{ext}(q,t)$ appears as a function $q$ and we need to compute its Jacobian w.r.t $q$. This computation needs the computation of
\begin{equation}
  \label{eq:116}
  \nabla_{p} M_{ext}(q,t) = \nabla_{p} \Phi(p) m_{ext}(t)
\end{equation}
Let us compute first
\begin{equation}
  \label{eq:117}
  \Phi(p) m_{ext}(t)  =
  \begin{bmatrix}
    (1-2 p_2^2- 2 p_3^2)m_{ext,1} + 2(p_1p_2-p_3p_0)m_{ext,2} + 2(p_1p_3+p_2p_0)m_{ext,3}\\
    2(p_1p_2+p_3p_0)m_{ext,1}  +(1-2 p_1^2- 2 p_3^2)m_{ext,2} + 2(p_2p_3-p_1p_0)m_{ext,3}\\
    2(p_1p_3-p_2p_0)m_{ext,1}  + 2(p_2p_3+p_1p_0)m_{ext,2}  + (1-2 p_1^2- 2 p_2^2)m_{ext,3}\\
  \end{bmatrix}
\end{equation}
Then we get
\begin{equation}
  \label{eq:118}
  \begin{array}{l}
  \nabla_{p} \Phi(p) m_{ext}(t)  =\\
  \begin{bmatrix}
    -2 p_3 m_{ext,2} + 2 p_2 m_{ext,3} & 2p_2 m_{ext,2}+2 p_3 m_{ext,3}  & -4 p_2 m_{ext,1} +2p_1 m_{ext,2}+2 p_0 m_{ext,3} & -3 p_3 m_{ext,1} -2p_0 m_{ext,2} +2 p_1m_{ext,3}  \\
    2p_3 m_{ext,1} -2p_1m_{ext,3}  & 2p_2m_{ext,1} -4p_1 m_{ext,2} -2p_1 m_{ext,3} & & &  \\
  \end{bmatrix}
  \end{array}
\end{equation}


\subsection{Siconos implementation}

The expression:~$\mathcal R_{free}(v^\tau_{k+1}) = M(v-v_k) - h  \theta F(t_{k+1}, q(v^\tau_{k+1}),v^\tau_{k+1}) - h (1- \theta) F(t_{k}, q_{k},v_{k})$ is computed in {\tt MoreauJeanOSI::computeResidu()} and saved in {\tt ds->workspace(DynamicalSystem::freeresidu)}


The expression:~$\mathcal R(v^\tau_{k+1}) =\mathcal R_{free}(v^\tau_{k+1}) - h (1- \theta) F(t_{k}, q_{k},v_{k}) - H^\top(\bar q_k) Q_{k+1} - G^\top(\bar q_k) P_{k+1}  $ is computed in {\tt MoreauJeanOSI::computeResidu()} and saved in {\tt ds->workspace(DynamicalSystem::free)}.
\begin{ndrva}
  really a bad name for the buffer {\tt ds->workspace(DynamicalSystem::free)}. Why we are chosing this name ? to save some memory ?
\end{ndrva}


The expression:~$v_{free}  = v^{\tau}_{k+1} - W  \mathcal R_{free}(v^\tau_{k+1})$ is compute in {\tt MoreauJeanOSI::computeFreeState()} and saved in {\tt d->workspace(DynamicalSystem::free)}.


The computation:~ $v^{\tau+1}_{k+1} = v_{free} + W   H^\top(\bar q_k) Q^{\tau+1}_{k+1} + W G^\top(\bar q_k) P^{\tau+1}_{k+1}$ is done in {\tt MoreauJeanOSI::updateState} and stored in {\tt d->twist()}.\\


%%% Local Variables:
%%% mode: latex
%%% TeX-master: "DevNotes"
%%% End: