pinocchio  2.2.1-dirty
2-invgeom.md
1 # 2) Grasp an object (aka inverse Geometry)
2 
3 ## Objectives
4 
5 The main objective of the first tutorial is to compute a configuration
6 of the robot minimizing a cost (maximizing a reward) and respecting some
7 given constraints. Additionally, the objective is to have a first hands
8 on the difficulties of working outside of real vector spaces and to
9 consider what are the guesses that are taken by an optimal solver.
10 
11 ## 2.0) Technical prerequisites
12 
13 ### Python SciPy and MatplotLib
14 
15 You will need the two Python libraries: *SciPy* (scientific Python) and
16 *MatPlotLib* (plot mathematical data).
17 
18 SciPy can be installed by `sudo apt-get install python-scipy`.
19 Examples of calls of
20 these two functions are given below. We will use both solvers with
21 numerical (finite-differencing) differentiation, to avoid the extra work
22 of differencing the cost and constraint functions by hand. In general,
23 it is strongly advice to first test a numerical program with finite
24 differencing, before implementing the true derivatives only if needed.
25 In any case, the true derivatives must always be checked by comparing
26 the results with the finite differentiation.
27 
28 Additionally, the provided implementation of BFGS allows the user to
29 provide a callback function and track the path taken by the solver, but
30 does not provide the possibility to specify constraints (constraints can
31 be added as penalty functions in the cost, but this requires additional
32 work). The constrained least-square implementation allows the user to
33 specify equality and inequality constraints, but not the callback. In
34 the following, start to use BFGS before moving to the constrained
35 least-square only when constraints are really needed.
36 
37 ```py
38 # Example of use a the optimization toolbox of SciPy.
39 
40 import numpy as np
41 from scipy.optimize import fmin_bfgs, fmin_slsqp
42 
43 def cost(x):
44  '''Cost f(x, y) = x² + 2y² - 2xy - 2x '''
45  x0, x1 = x
46  return -(2 * x0 * x1 + 2 * x0 - x0 ** 2 - 2 * x1 ** 2)
47 
48 def constraint_eq(x):
49  ''' Constraint x³ = y '''
50  return np.array([ x[0] ** 3 - x[1] ])
51 
52 def constraint_ineq(x):
53  '''Constraint x >= 2, y >= 2'''
54  return np.array([ x[0] - 2, x[1] - 2 ])
55 
56 class CallbackLogger:
57  def __init__(self):
58  self.nfeval = 1
59 
60  def __call__(self,x):
61  print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
62  self.nfeval += 1
63 
64 x0 = np.array([0.0, 0.0]) # Optimize cost without any constraints in BFGS, with traces.
65 xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())
66 print('*** Xopt in BFGS =', xopt_bfgs)
67 
68 # Optimize cost without any constraints in CLSQ
69 
70 xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)
71 print('*** Xopt in LSQ =', xopt_lsq)
72 
73 # Optimize cost with equality and inequality constraints in CLSQ
74 
75 xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)
76 print('*** Xopt in c-lsq =', xopt_clsq)
77 ```
78 
79 Take care that all *SciPy*
80 always works with vectors represented as 1-dimensional array, while
81 Pinocchio works with vectors represented as matrices (which are in fact
82 two-dimensional arrays, with the second dimension being 1). You can pass
83 from a SciPy-like vector to a Pinocchio-like vector using:
84 
85 ```py
86 import numpy as np
87 x = np.array([1.0, 2.0, 3.0])
88 q = np.matrix(x).T
89 x = q.getA()[:, 0]
90 ```
91 
92 The second library *MatPlotLib* plots values on a 2D graph.
93 A tutorial is available [here](https://www.labri.fr/perso/nrougier/teaching/matplotlib/).
94 An example is provided below.
95 
96 ```py
97 import numpy as np
98 import matplotlib.pyplot as plt
99 # In plt, the following functions are the most useful:
100 # ion, plot, draw, show, subplot, figure, title, savefig
101 
102 # For use in interactive python mode (ipthyon -i)
103 interactivePlot = False
104 
105 if interactivePlot:
106  plt.ion() # Plot functions now instantaneously display, shell is not blocked
107 
108 # Build numpy array for x axis
109 x = 1e-3 * np.array(range(100))
110 # Build numpy array for y axis
111 y = x ** 2
112 
113 fig = plt.figure()
114 ax = fig.add_subplot('111')
115 ax.plot(x, y)
116 ax.legend(("x^2", ))
117 
118 if not interactivePlot:
119  # Display all the plots and block the shell.
120  # The script will only ends when all windows are closed.
121  plt.show()
122 ```
123 
124 ### Robots
125 
126 We mostly use here the model UR5, used in the first lab. Refer to
127 the instructions of Lab 1 to load it.
128 
129 Optionally, we might want to use a more complex robot model. Romeo is a
130 humanoid robot developed by the French-Japanese company Aldebaran
131 Robotics. It has two legs, two arms and a head, for a total of 31 joints
132 (plus 6DOF on the free flyer). The description of Romeo can be obtained with:
133 
134 ```
135 sudo apt install robotpkg-romeo-description
136 ```
137 
138 Romeo can be loaded with:
139 
140 ```py
141 from os.path import join
142 import pinocchio as se3
143 from pinocchio.romeo_wrapper import RomeoWrapper
144 
145 PKG = '/opt/openrobots/share'
146 URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
147 
148 robot = RomeoWrapper(URDF, [PKG]) # Load urdf model
149 robot.initDisplay(loadModel=True)
150 ```
151 
152 Additionally, the index of right
153 and left hands and feet are stored in `robot.rh`, `robot.lh`, `robot.rf` and
154 `robot.lf`.
155 
156 ## 2.1) Position the end effector
157 
158 The first tutorial is to position (i.e. translation only) the end
159 effector of a manipulator robot to a given position. For this first
160 part, we will use the fixed serial-chain robot model.
161 
162 Recall first that the position (3D) of the joint with index `i` at
163 position `q` can be access by the following two lines of code:
164 
165 ```py
166 # Compute all joint placements and put the position of joint "i" in variable "p".
167 import pinocchio as se3
168 se3.forwardKinematics(robot.model, robot.data, q)
169 p = robot.data.oMi[i].translation
170 ```
171 
172 #### Question 1
173 Using this, build a cost function to be the norm of the
174 difference between the end-effector position `p` and a desired position
175 `pdes`. The cost function is a function that accepts as input an
176 1-dimensional array and return a float.
177 
178 #### Question 2
179 Then use `fmin_bfgs` to find a configuration `q` with the
180 end effector at position `pdes`.
181 
182 #### Question 3
183 Finally, implements a callback function that display in
184 Gepetto-Viewer every candidate configuration tried by the solver.
185 
186 ## 2.2) Approaching the redundancy (optionnal)
187 
188 The manipulator arm has 6 DOF, while the cost function only constraints
189 3 of them (the position of the end effector). A continuum of solutions
190 then exists. The two next questions are aiming at giving an intuition of
191 this continuum.
192 
193 #### Question 4
194 Sample several configurations respecting `pdes` by giving
195 various initial guesses to the solver. Store this sampling of solutions
196 in a list, then display this list in Gepetto-Viewer, each configuration
197 begin displayed during 1 second (pause of 1 seconds can be obtained
198 using: `import time; time.sleep(1))`.
199 
200 A configurations in this continuum can then be selected with particular
201 properties, like for example being the closest to a reference
202 configuration, or using some joints more than the others, or any other
203 criterion that you can imagine.
204 
205 #### Question 5
206 Sum a secondary cost term to the first positioning cost, to
207 select the posture that maximizes the similarity (minimizes the norm of
208 the difference) to a reference posture. The relative importance of the
209 two cost terms can be adjusted by weighting the sum: find the weight so
210 that the reference position is obtained with a negligible error (below
211 millimeter) while the posture is properly taken into account.
212 
213 ## 2.3) Placing the end-effector
214 
215 The next step is to find a configuration of the robot so that the end
216 effector respects a reference placement, i.e. position and orientation.
217 The stake is to find a metric in \f$SE(3)\f$ to continuously quantify the
218 distance between two placements. There is no canonical metric in \f$SE(3)\f$,
219 i.e. no absolute way of weighting the position with respect to the
220 orientation. Two metrics can be considered, namely the log in \f$SE(3)\f$ or
221 in \f$R^3 \times SO(3)\f$. The tutorial will guide you through the first choice.
222 
223 The \f$SE(3)\f$ and \f$SO(3)\f$ logarithm are implemented in Pinocchio in the `explog`
224 module.
225 
226 ```py
227 from pinocchio.explog import log
228 from pinocchio import SE3
229 nu = log(SE3.Random())
230 nu_vec = nu.vector
231 ```
232 
233 #### Question 6
234 Solve for the configuration that minimizes the norm of the
235 logarithm of the difference between the end effector placement and the
236 desired placement.
237 
238 Optionally, try other metrics, like the log metric of \f$R^3 \times SO(3)\f$, or
239 the Froebenius norm of the homogeneous matrix.
240 
241 ## 2.4) Working with a mobile robot (optionnal)
242 
243 Until now, the tutorial only worked with a simple manipulator robot,
244 i.e. whose configuration space is a real vector space. Consider now the
245 humanoid robot, whose first joint is a free joint: it has 6 degrees of
246 freedom (3 rotations, 3 translations) but its configuration vector is
247 dimension 7. You can check it with `robot.model.nq`, that stores the
248 dimension of the configuration, and `robot.model.nv`, that stores the
249 dimension of the configuration velocity, i.e. the number of degrees of
250 freedom. For the humanoid, `nq = nv + 1`.
251 
252 Indeed, the configuration coefficients 3 to 7 are indeed representing a
253 quaternion. The additional constraint is that these 4 coefficients must
254 be normalize.
255 
256 #### Question 7
257 Display a configuration of the robot for which the norm of
258 the quaternion is bigger than one (e.g. 2.0). What happens?
259 
260 During the search, the solver must respect this constraint. A solution
261 is to make this constraint explicit in the numerical program. However,
262 we will start by an easier quick-and-dirty trick. With quaternions, the
263 trick is simply to normalize any invalid quaternions. In the cost
264 function, first normalize the quaternion before computing the cost due
265 to the end-effector placement. An additional term should also be added
266 to the cost function to avoid excessive drift of the quaternion norm, in
267 particular with the norm going to 0.
268 
269 #### Question 8
270 Use `fmin_bfgs` to compute a configuration respecting a
271 given placement with the humanoid model, by normalizing the quaternion
272 at each step.
273 
274 #### Question 9 (harder)
275 Do the same with the solver C-LSQ `fmin_slsqp`,
276 with the explicit constraint that the norm of the quaternion must be 1.
277 
278 ## 2.5) Configuration of a parallel robot
279 
280 A parallel robot is composed of several kinematic chains (called the
281 robot legs) that are all attached to the same end effector. This imposes
282 strict constraints in the configuration space of the robot: a
283 configuration is valide iff all the legs meets the same end-effector
284 placement. We consider here only the geometry aspect of parallel robots
285 (additionnally, some joints are not actuated, which causes additional
286 problems).
287 
288 The kinematic structure of a paralel robot indeed induces loops in the
289 joint connection graph. In Pinocchio, we can only represents (one of)
290 the underlying kinematic tree. The loop constraints have to be handled
291 separately. An example that loads 4 manipulator arms is
292 [available here](ur5x4_8py_source.html). Each leg i (for i=0,1,2,3)
293 of the robot is loaded in the list `robots[i]`. The loop constraints are
294 that the relative placement of every leg end-effector must stay the same
295 that in the initial configuration given as example in the above file.
296 
297 #### Question 10
298 Consider now that the orientation of the tool plate is
299 given by the quaternion `Quaternion(0.7, 0.2, 0.2, 0.6)`, with the
300 translation that you like. Find using the above optimization routines
301 the configuration of each robot leg so that the loop constraints are all
302 met.