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
|
\subsection{Velocity formulation}
The first step consists in doing a velocity formulation of the system:
\begin{equation}
\label{NE_Dyn1}
\begin{array}{l}
M \dot v = F_{ext}+B \lambda \\
\dot q = T v\\
y=h(q) \\
NSLAW(y,\lambda,...)\\
\end{array}
\end{equation}
The constraint $\dot q = T v$ is suffisiant to keep a normal quaternion. Because of the speed formulation, $h(q)$ could violate the NSLAW. A solution coul be to add a formulation in position. We must underline that the constraints $\mid Q \mid = 1$ is implicit in this system. Endeed, the direction $\dot q = T v$ is tangential to the sphere.
\subsection{Posion formulation}
It consists in writting a position formulation on the system:
\begin{equation}
\label{NE_Dyn1}
\begin{array}{l}
h(q) = \left(\begin{array}{l}
HI(q)\\HE(q)
\end{array}\right)
\end{array}
\end{equation}
\subsubsection{Approach using q}
We are looking for $q_1$ from $q_0$:\\
\begin{equation}
q_1=q_0+\nabla HI \Lambda _I + \nabla HE \Lambda _E
\end{equation}
Assume that $h(q_0)$ doesn't satisfy the constraints, ie $ HI(q_0) \ngeq 0$ or $ HE(q_0) \neq 0)$. Linearize $h$ leads to:\\
\begin{equation}
0 \leq HI(q_0) + \nabla ^t HI (\nabla HI \Lambda _I + \nabla HE \Lambda _E) \bot \Lambda _I \geq 0
\end{equation}
\begin{equation}
0=HE(q_0) + \nabla ^t HE (\nabla HI \Lambda _I + \nabla HE \Lambda _E)
\end{equation}
The getting system could be written has a MLCP:
\begin{equation}
C \ni h(q_0)+ \nabla ^t h(\nabla h\Lambda) , \Lambda \in C^*
\end{equation}
In the case of a quaternion $Q$ for the rotation representation, it is noteworthy that this system doesn't deal with the constraints $\mid Q \mid = 1$. Thus, the direction $(q_1,q_0)$ can be normal to this constraint, in that case this approach doesn't work. (It happens in practice) The solution that consists in normaliaed q after this formulation is not convenient because, it could be incompatible with $\mid Q \mid = 1$. A better approach is to add this constraint.
The constraint $\mid Q \mid = 1$ in the system HE:
\begin{equation}
\tilde {HE}(q)= \left(\begin{array}{l}
HE(q)\\
\mid Q \mid -1
\end{array}\right)
\end{equation}
The formulation described above can be done.
\subsubsection{Approach using V}
It consists in building the OSNSP using $CT$ instead of $C$.
\begin{equation}
\label{NE_projV}
h(q_1) = h(q_0)+\nabla ^t H \delta q
\end{equation}
ie:
\begin{equation}
\label{NE_projV}
h(q_1) = h(q_0)+\nabla ^t H T V
\end{equation}
We are looking for $q_1$ such that:
\begin{equation}
q_1-q_0 = \nabla H \Lambda
\end{equation}
We have
\[\delta q=TV, \qquad ^tT\delta q=^tTTV, \qquad(^tTT)^{-1}~^tT\delta q=V\]
ie
\begin{equation}
h(q_1)=h(q_0)+^t\nabla _q hT(^tTT)^{-1}~^tT\nabla _q h\Lambda
\end{equation}
With $C=^t\nabla _q h$ leading to the prolem:
\begin{equation}
K \ni h(q_0)+ CT~ (^tTT)^{-1}~ ^t(CT) \Lambda \in K^*
\end{equation}
|
