1 # 2) Grasp an object (aka inverse Geometry)
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.
11 ## 2.0) Technical prerequisites
13 ### Python SciPy and MatplotLib
15 You will need the two Python libraries: *SciPy* (scientific Python) and
16 *MatPlotLib* (plot mathematical data).
18 SciPy can be installed by `sudo apt-get install python-scipy`.
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.
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.
38 # Example of use a the optimization toolbox of SciPy.
41 from scipy.optimize import fmin_bfgs, fmin_slsqp
44 '''Cost f(x, y) = x² + 2y² - 2xy - 2x '''
46 return -(2 * x0 * x1 + 2 * x0 - x0 ** 2 - 2 * x1 ** 2)
49 ''' Constraint x³ = y '''
50 return np.array([ x[0] ** 3 - x[1] ])
52 def constraint_ineq(x):
53 '''Constraint x >= 2, y >= 2'''
54 return np.array([ x[0] - 2, x[1] - 2 ])
61 print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
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)
68 # Optimize cost without any constraints in CLSQ
70 xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)
71 print('*** Xopt in LSQ =', xopt_lsq)
73 # Optimize cost with equality and inequality constraints in CLSQ
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)
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:
87 x = np.array([1.0, 2.0, 3.0])
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.
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
102 # For use in interactive python mode (ipthyon -i)
103 interactivePlot = False
106 plt.ion() # Plot functions now instantaneously display, shell is not blocked
108 # Build numpy array for x axis
109 x = 1e-3 * np.array(range(100))
110 # Build numpy array for y axis
114 ax = fig.add_subplot('111')
118 if not interactivePlot:
119 # Display all the plots and block the shell.
120 # The script will only ends when all windows are closed.
126 We mostly use here the model UR5, used in the first lab. Refer to
127 the instructions of Lab 1 to load it.
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:
135 sudo apt install robotpkg-romeo-description
138 Romeo can be loaded with:
141 from os.path import join
142 import pinocchio as se3
143 from pinocchio.romeo_wrapper import RomeoWrapper
145 PKG = '/opt/openrobots/share'
146 URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
148 robot = RomeoWrapper(URDF, [PKG]) # Load urdf model
149 robot.initDisplay(loadModel=True)
152 Additionally, the index of right
153 and left hands and feet are stored in `robot.rh`, `robot.lh`, `robot.rf` and
156 ## 2.1) Position the end effector
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.
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:
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
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.
179 Then use `fmin_bfgs` to find a configuration `q` with the
180 end effector at position `pdes`.
183 Finally, implements a callback function that display in
184 Gepetto-Viewer every candidate configuration tried by the solver.
186 ## 2.2) Approaching the redundancy (optionnal)
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
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))`.
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.
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.
213 ## 2.3) Placing the end-effector
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.
223 The \f$SE(3)\f$ and \f$SO(3)\f$ logarithm are implemented in Pinocchio in the `explog`
227 from pinocchio.explog import log
228 from pinocchio import SE3
229 nu = log(SE3.Random())
234 Solve for the configuration that minimizes the norm of the
235 logarithm of the difference between the end effector placement and the
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.
241 ## 2.4) Working with a mobile robot (optionnal)
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`.
252 Indeed, the configuration coefficients 3 to 7 are indeed representing a
253 quaternion. The additional constraint is that these 4 coefficients must
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?
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.
270 Use `fmin_bfgs` to compute a configuration respecting a
271 given placement with the humanoid model, by normalizing the quaternion
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.
278 ## 2.5) Configuration of a parallel robot
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
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.
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