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