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