pinocchio  2.3.1-dirty
e-lie.md
1 
2 # Dealing with Lie group geometry
3 
4 Pinocchio relies heavily on Lie groups and Lie algebras to handle motions and more specifically rotations.
5 For this reason it supports the following special groups \\( SO(2), SO(3), SE(2), SE(3) \\) and implements their associated algebras
6 \f$ \mathfrak{se}(2) , \mathfrak{se}(3) \f$.
7 It has various applications like representing the motion of a robot free flyer joint (typically the base of a mobile robot),
8 or the motion of the robot links. The later is particularly useful for collision detection.
9 It is also interesting to have general vector space over which a Lie algebra is defined.
10 
11 
12 ## Using \\( SE(2) \\) with pinocchio in C++
13 
14 As a motivating example let us consider a mobile robot evolving in a plane \f$(\mathbb{R}^2 \times \mathbb{S}^1 \f$).
15 ![SE2MotivatingExample](SE2MotivatingExample.svg)
16 
17 The robot starts at position \f$ pose_s = (x_s,y_s,\theta_s) \f$ and after a rigid motion
18 \f$ \delta_u=(\delta x,\delta y,\delta \theta) \f$
19 it finishes
20 at \f$ pose_g = (x_{g},y_{g},\theta_{g})\f$.
21 It is possible to instantiate the corresponding \\(SE(2)\\) objects using:
22 
23 \code
24  typedef double Scalar;
25  enum {Options = 0};
26 
27  SpecialEuclideanOperationTpl<2,Scalar,Options> aSE2;
28  SpecialEuclideanOperationTpl<2,Scalar,Options>::ConfigVector_t pose_s,pose_g;
29  SpecialEuclideanOperationTpl<2,Scalar,Options>::TangentVector_t delta_u;
30 \endcode
31 You can change Scalar by another type such as float.
32 
33 
34 In this example, \f$ pose_s=(1,1,\pi/4)\f$ and \f$ pose_g=(3,1,-\pi/2) \f$ and we want to compute
35 \f$ \delta_u \f$
36 \code
37  pose_s(0) = 1.0; pose_s(1) = 1.0;
38  pose_s(2) = cos(M_PI/4.0); pose_s(3) = sin(M_PI/4.0);
39  pose_g(0) = 3.0; pose_g(1) = -1.0;
40  pose_g(2) = cos(-M_PI/2.0); pose_g(3) = sin(-M_PI/2.0);
41 
42  aSE2.difference(pose_s,pose_g,delta_u);
43  std::cout << delta_u << std::endl;
44 \endcode
45 
46 aSE2 is used to compute the difference between two configuration vectors representing the two poses. Note that the rotation is represented by two numbers \f$ sin(\theta),cos(\theta)\f$ which is also a \f$ SO(2) \f$ object.
47 The difference lies in the tangent space of \f$ SE(2)\f$ and is representend by a vector of 3 reals.
48 Therefore the output is:
49 
50 \code
51  3.33216
52 -1.38023
53 -2.35619
54 \endcode
55 
56 Note that the linear part is not following a straight path, it also takes into account
57 that the system is rotating.
58 
59 We can verify that this is the appropriate motion by integrating:
60 \code
61  SpecialEuclideanOperationTpl<2,Scalar,Options>::ConfigVector_t pose_check;
62 
63  aSE2.integrate(pose_s,delta_u,pose_check);
64  std::cout << pose_check << std::endl;
65 \endcode
66 
67 The result is indeed:
68 
69 \code
70 3
71 -1
72  0
73 -1
74 \endcode
75 
76 ## Using \f$ SE(3) \f$ with pinocchio in C++
77 
78 Our mobile robot is not in a plane but in a 3-dimensional space. So let's consider a object in our physical space. This is actually almost the same case, we want the object from one position to an other position. The difficulty lies in the fact that we now have three dimensions so the object has six degrees of freedom, three corresponding to its translation and three to its rotation.
79 
80 ![SE3MotivatingExample](SE3Example1.jpg)
81 
82 It is also possible to instantiate the corresponding object which is now a \f$ SE(3) \f$ object using the same algorithm and changing the dimension parameter:
83 
84 \code
85  typedef double Scalar;
86  enum {Options = 0};
87 
88  SpecialEuclideanOperationTpl<3,Scalar,Options> aSE3 ;
89  SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pose_s,pose_g;
90  SpecialEuclideanOperationTpl<3,Scalar,Options>::TangentVector_t delta_u ;
91 \endcode
92 
93 In this example, \f$ pose_s=(1,1,1,\pi/2,\pi/4,\pi/8)\f$ and \f$ pose_g=(4,3,3,\pi/4,\pi/3, -\pi) \f$. For the starting position, there is first a rotation around the y-axis then the x-axis and finally the z-axis. For the final position, the rotations are in this order, x-axis, y-axis, z-axis. We want to compute \f$ \delta_u \f$.
94 
95 - For the first pose, we have the three rotations matrices for each rotation :
96 
97  \f$ R_{x_s} =
98  \begin{bmatrix} 1 &0 &0 \\ 0 &cos(\pi/8) &-sin(\pi/8) \\0 &sin(\pi/8) &cos(\pi/8) \end{bmatrix} \ \ R_{y_s} = \begin{bmatrix} cos(\pi/4) &0 &sin(\pi/4) \\ 0 &1 &0 \\-sin(\pi/4) &0 &cos(\pi/4) \end{bmatrix} \ \ R_{z_s} = \begin{bmatrix} cos(\pi/2) &-sin(\pi/2) &0 \\sin(\pi/2) &cos(\pi/2) &0 \\ 0 &0 &1\end{bmatrix} \f$
99 
100  Therefore, the complete rotation is:
101 
102  \f$ R_{pose_s} = R_{s_z} * R_{s_x} * R_{s_y} = \begin{bmatrix} 0 &-1 &0 \\ cos(\pi/4)*cos(\pi/8) + sin(\pi/4) * sin(\pi/8) &0 &sin(\pi/4) * cos(\pi/8) - cos(\pi/4) * sin(\pi/8) \\ sin(\pi/8) * cos(\pi/4) - cos(\pi/8) * sin(\pi/4) &0 &sin(\pi/4) * sin(\pi/8) + cos(\pi/4) * cos(\pi/8) \end{bmatrix} \f$
103 
104 - For the second one, we have:
105 
106  \f$ R_{x_g} =
107  \begin{bmatrix} 1 &0 &0 \\ 0 &cos(\pi/4) &-sin(\pi/4) \\0 &sin(\pi/4) &cos(\pi/4) \end{bmatrix} \ \ R_{y_g} = \begin{bmatrix} cos(\pi/3) &0 &sin(\pi/3) \\ 0 &1 &0 \\ -sin(\pi/3) &0 &cos(\pi/3) \end{bmatrix} \ \ R_{z_g} = \begin{bmatrix} cos(-\pi) &-sin(-\pi) &0 \\ sin(-\pi) &cos(-\pi) &0 \\ 0 &0 &1\end{bmatrix} \f$
108 
109  The complete rotation is:
110 
111  \f$ R_{pose_g} =
112  \begin{bmatrix} -cos(\pi/3) &-sin(\pi/3) * sin(\pi/4) &-cos(\pi/4) * sin(\pi/3) \\ 0 &-cos(\pi/4) &sin(\pi/4) \\ -sin(\pi/3) &sin(\pi/4) * cos(\pi/3) &cos(\pi/3) * cos(\pi/4) \end{bmatrix} \f$
113 
114 
115 To compute \f$ \delta_u \f$ using Pinocchio we need to transform \f$ R_{pose_s} \f$ and \f$ R_{pose_g} \f$ matrices into quaternions using:
116 
117 \code
118  float s = 0.5f / sqrtf(trace+ 1.0f);
119  q.x = ( R[2][1] - R[1][2] ) * s;
120  q.y = ( R[0][2] - R[2][0] ) * s;
121  q.z = ( R[1][0] - R[0][1] ) * s;
122  q.w = 0.25f / s;
123 \endcode
124 
125 The quaternions components are:
126 - For the first rotation
127  \code
128  0.69352
129  -0.13795
130  0.13795
131  0.69352
132  \endcode
133 
134 - For the second one
135  \code
136  0.191342
137  -0.46194
138  0.331414
139  0.800103
140  \endcode
141 
142 For each pose we have now a mathematical object with seven components and both are normalized. As for the \f$ SE(2) \f$ example we compute \f$ \delta_u \f$ using:
143 
144 \code
145  pose_s(0) = 1.0; pose_s(1) = 1.0;
146  pose_s(2) = 1 ; pose_s(3) = -0.13795 ;
147  pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352;
148  pose_g(0) = 4; pose_g(1) = 3 ;
149  pose_g(2) = 3 ; pose_g(3) = -0.46194;
150  pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342;
151 
152  aSE3.difference(pose_s,pose_g,delta_u);
153  std::cout << delta_u << std::endl;
154 \endcode
155 
156 The difference lies in the tangent space of \f$ SE(3)\f$ and is represented by a vector of 6 reals which is:
157 
158 \code
159  -1.50984
160  -3.58755
161  2.09496
162  -0.374715
163  0.887794
164  0.86792
165 \endcode
166 
167 The three first values are linear and the three last are velocities.
168 
169 To verify it is the good solution, we integrate:
170 
171 \code
172  SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pose_check;
173 
174  aSE3.integrate(pose_s,delta_u,pose_check);
175  std::cout << pose_check << std::endl;
176 \endcode
177 
178 Indeed, we find :
179 
180 \code
181  4
182  3
183  3
184  -0.46194
185  0.331414
186  0.800103
187  0.191234
188 \endcode
189 
190 
191 
192 ## Using interpolation to plot a trajectory with pinocchio in C++
193 
194 Assuming that we want to make the robot pass through known positions, we can use interpolations to plot a trajectory.
195 
196 The problem is an interpolation such as Lagrange's one only takes into account translations whlie the robot interact with its environment by performing translations and rotations.
197 
198 A possibility is to use the \f$ \delta_{theta} \f$ method by using quaternions. The method is simple, we just vary the angle, the scalar component of the quaternion, with very small variations.
199 
200 Let's consider the previous example, we can interpolate trajectory using:
201 \code
202  SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pole;
203  aSE3.interpolate(pose_s,pose_g,0.5f, pole);
204  std::cout << pole << std::endl;
205 \endcode
206 
207 The output corresponds to the middle of the trajectory and is:
208 \code
209  2.7486
210  1.4025
211  2.22461
212  -0.316431
213  0.247581
214  0.787859
215  0.466748
216 \endcode
217 
218 
219