1\subsection{Gradient computation, case of NewtonEuler with quaternion}
2
3In the section, $q$ is the quaternion of the dynamical system.
4
5\begin{figure}[h]
6  \centering
7
8  %\input{./Figures/NewtonEulerImpact.pstex_t}
9  \input{./Figures/NewtonEulerImpact.pdf_t}
10
11  \caption{Impact of one DS.}
12  \label{figCase}
13\end{figure}
14
15The normal vector $N$ is view as a constant.
16\[~\tilde h(q)=P_c(\frac{q}{\|q\|})\]
17\[^t \nabla h(q)(\delta q) = \lim _{e \to 0}\frac{(\tilde h (q+e\delta q)-\tilde h (q)).N}{e}  \]
18
19$\nabla _q h$ consist in computing $P_c(\frac{q+\delta q}{\|q+\delta q\|})-P_c(q)$.
20\[GP(q)=qG_0P_0~^cq\]
21\[GP(\frac{q+\delta q}{\|q+\delta q\|})=(q+\delta q)G_0P_0~^c(q+\delta q)\frac{1}{\|q+\delta q\|^2}\]
22\[=(q+\delta q)~^cqGP(q)q~^c(q+\delta q)\frac{1}{\|q+\delta q\|^2}\]
23\[=((1,0,0,0)+\delta q~^cq)GP(q)((1,0,0,0)+q~^c\delta q)\frac{1}{\|q+\delta q\|^2}\]
24\[=GP(q)+\delta q~^cqGP(q) + GP(q)q~^c\delta q+0(\delta q)^2\frac{1}{\|q+\delta q\|^2}\]
25So, because G is independant of $q$:
26\[P(\frac{q+\delta q}{\|q+\delta q\|})-P(q)=qGP(\frac{q+\delta q}{\|q+\delta q\|})-GP(q)=\delta q~^cqGP(q) + GP(q)q~^c\delta q+0(\delta q)^2 + GP(q)\frac{1}{\|q+\delta q\|^2}\]
27For the directional derivation, we chose $\delta q = \epsilon * (1,0,0,0)$. using a equivalent to $\frac{1}{1+\epsilon}$
28\[\lim_{\epsilon \to 0}\frac{P(\frac{q+\delta q}{\|q+\delta q\|})-P(q)}{\epsilon}=~^cqGP(q) + GP(q)q-2q_iGP(q)\]
29For the directional derivation, we chose $\delta q = \epsilon * (0,1,0,0)=\epsilon * e_i$
30\[\lim_{\epsilon \to 0}\frac{P(\frac{q+\delta q}{\|q+\delta q\|})-P(q)}{\epsilon}=e_i~^cqGP(q) - GP(q)qe_i-2q_iGP(q)\]
31Application to the NewtonEulerRImpact:
32\[H:\mathbb{R}^7 \to \mathbb{R}\]
33\[\nabla _q H \in \mathcal{M}^{1,7}\]
34\[\nabla _q H =\left(\begin{array}{c} N_x\\N_y\\N_z\\
35(~^cqGP(q) + GP(q)q-2q_0GP(q)).N\\
36(e_2~^cqGP(q) - GP(q)qe_2-2q_1GP(q)).N\\
37(e_3~^cqGP(q) - GP(q)qe_3-2q_2GP(q)).N\\
38(e_4~^cqGP(q) - GP(q)qe_4-2q_3GP(q)).N\\
39\end{array}\right)\]
40\subsection{Ball case}
41It is the case where $GP=-N$:
42for $e2$:
43\[(0,1,0,0).(q_0,-\underline p).(0,-N)=\]
44\[\left(\left(\begin{array}{c}1\\0\\0\end{array}\right).\underline p,\left(\begin{array}{c}q_0\\0\\0\end{array}\right) -\left(\begin{array}{c}1\\0\\0\end{array}\right)*\underline p \right).(0,-N)=\]
45\[\left(?, -\underline p_x~N-\left(\left(\begin{array}{c}q_0\\0\\0\end{array}\right)- \left(\begin{array}{c}1\\0\\0\end{array}\right)*\underline p \right)*N\right)=\]
46and:
47\[(0,-N).(q_0,\underline p).(0,1,0,0)=\]
48\[(N.\underline p,-q_0N-N*\underline p).(0,1,0,0)=\]
49\[\left(?,(N.\underline p)\left(\begin{array}{c}1\\0\\0\end{array}\right) + \left(\begin{array}{c}1\\0\\0\end{array}\right)*(q_0N+N*\underline p)\right)=\]
50\[\left(?,(N.\underline p)\left(\begin{array}{c}1\\0\\0\end{array}\right)+q_0 \left(\begin{array}{c}1\\0\\0\end{array}\right)*N+\left(\begin{array}{c}1\\0\\0\end{array}\right)*(N*\underline p)\right)\]
51sub then and get the resulting vector.N:
52\[\left[ -\underline p_x~N -N.\underline p~\left(\begin{array}{c}1\\0\\0\end{array}\right)+()*N-\left(\begin{array}{c}1\\0\\0\end{array}\right)*(N*\underline p)\right].N=\]
53\[-\underline p_x-N_xN.\underline p+0-(\left(\begin{array}{c}1\\0\\0\end{array}\right)*(N*\underline p)).N=\]
54  using $a*(b*c)=b(a.c)-c(a.b)$ leads to
55  \[-q_1-N_xN.\underline p-(q_1~N-N_x~\underline p).N=\]
56\[-q_1-N_xN.\underline p-q_1+N_xN.\underline p=-2q_1\]
57for $e1=(1,0,0,0)$:
58\[(q_0,-\underline p).(0,-N)=(?,-q_0N+\underline p*N)\]
59\[(0,-N).(q_0,\underline p)=(?,-q_0N-\underline p*N)\]
60So
61\[\nabla _q H =\left(\begin{array}{c} N_x\\N_y\\N_z\\
620\\
630\\
640\\
650\\
66\end{array}\right)\]
67
68\subsection{Case FC3D: using the local frame and momentum}
69
70\[\left(\begin{array}{c}m \dot V\\I \dot \Omega + \Omega I \Omega \end{array}\right)= \left(\begin{array}{c}Fect+R\\Mext _{R_{obj}} + (R*PG) _{R_{obj}} \end{array}\right)\]
71  with * vectoriel product, $R$ reaction in the globla frame. $P$ the point of contact.
72  $r$ is the reaction in the local frame.  $M_{R_{obj}toR_{abs}}=M_{R_{abs}toR_{obj}}^t r=R$ with:
73  \[M_{R_{C}toR_{abs}}=\left(\begin{array}{ccc} nx&t_1x&t_2x \\ny&t_1y&t_2y\\nz&t_1z&t_2z \end{array}\right)\]
74  we have :
75  \[\left(\begin{array}{c}R\\(R*PG) _{R_{obj}}\end{array}\right)=\left(\begin{array}{c} I_3\\M_{R_{abs}toR_{obj}}N_{PG}\end{array}\right).R\]
76  \[=\left(\begin{array}{c} I_3\\M_{R_{abs}toR_{obj}}N_{PG}\end{array}\right).M_{R_{obj}toR_{abs}}r\]
77  \[ N_{PG}=\left(\begin{array}{ccc} 0&PG_z&-PG_y\\-PG_z&0&PG_x\\PG_y&-PG_X&0\end{array}\right)\]
78    that is:
79
80\[\left(\begin{array}{c}m \dot V\\I \dot \Omega + \Omega I \Omega \end{array}\right)=
81\left(\begin{array}{c} M_{R_{C}toR_{abs}} \\
82  M_{R_{abs}toR_{obj}}N_{PG}M_{R_{C}toR_{abs}}
83\end{array}\right) r\]
84So $jachqt=MN$
85
86\subsection{Case FC3D: using the local frame local velocities}
87\begin{figure}[h!]
88  \centering
89   \scalebox{0.6}{
90  \input{./Figures/SolideContact.pdf_t}
91  %   \input{./Figures/SolideContact.pdf_t}
92  }
93  \caption{Two objects colliding.}
94  \label{figCase}
95\end{figure}
96
97
98We are looking for an operator named $CT$ such that:
99
100\[V_C=\left(\begin{array}{c} V_N \\ V_T \\ V_S \end{array}\right)_{R_{C}}=CT \left(\begin{array}{c} V_{G1}~_{R_{abs}} \\ \Omega_1~_{R_{obj1}} \\ V_{G2}~_{R_{abs}}\\ \Omega_2~_{R_{obj2}} \end{array}\right)\]
101
102\[V_c=V_{G1}~_{R_{abs}} + w_1 * G_1P~_{R_{abs}} -(V_{G2}~_{R_{abs}} + w_2 * G_1P~_{R_{abs}})\]
103where $w_1$ and $w_2$ are given in $R_{abs}$. We note $M_{R_{obj1}toR_{abs}}$ the matrice converting the object 1 coordinate to the absolute coordinate. We note $N_{GP}$ the matrice such that $w_1*G_1P~_{R_{abs}} = N_{GC} w_1$. Endly, we note $M_{R_{abs}toR_C}$ converting the absolute coordinate to the $R_C$ frame.
104we get:
105\[CT= M_{R_{abs}toR_C}   \left(\begin{array}{cccc} I_3 & N_{G_1C}M_{R_{obj1}toR_{abs}} & -I_3 & -N_{G_2C}M_{R_{obj2}toR_{abs}} \end{array}\right)\]
106
107\subsubsection{Expression of $M_{R_{obj1}toR_{abs}}$}
108Using quaternion, we get :
109\begin{equation}
110  \label{eq:newton_Mobjtoabs}
111M_{R_{obj1}toR_{abs}} = \left(\begin{array}{ccc} q \left(\begin{array}{c}1\\0\\0 \end{array}\right)~^cq & q \left(\begin{array}{c}  0\\1\\0 \end{array}\right)~ ^cq & q \left(\begin{array}{c}  0\\0\\1 \end{array}\right)~ ^cq  \end{array}\right)
112\end{equation}
113
114\subsubsection{Expression of $N_1$}
115\[N_{GC}=\left(\begin{array}{ccc} 0&G_1C_z&-G_1C_y\\-G_1C_z&0&G_1C_x\\G_1C_y&-G_1C_X&0\end{array}\right)\]
116
117%%% Local Variables:
118%%% mode: latex
119%%% TeX-master: "DevNotes"
120%%% End:
121