2_geometry_3d.ipynb 22.5 KB
Newer Older
Nicolas Mansard's avatar
Nicolas Mansard committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Direct and inverse geometry of 3d robots\n",
    "This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
17
    "import magic_donotload  # noqa: F401"
Nicolas Mansard's avatar
Nicolas Mansard committed
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "import numpy as np\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
36
    "from numpy.linalg import norm\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "from scipy.optimize import fmin_bfgs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematic tree in Pinocchio\n",
    "Let's now play with 3D robots. We will load the models from URDF files.\n",
    "\n",
    "*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format \".dae\". Both the URDF and the DAE files are available in the repository in the model directory. \n",
    "\n",
    "This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
60
    "# %load tp2/generated/simple_pick_and_place_1\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
61
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
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
121
122
123
124
125
126
127
    "robot = robex.load('ur5')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(robot.model)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the next steps, we are going to work with the RobotWrapper.\n",
    "\n",
    "Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory \"models\" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.\n",
    "\n",
    "Here are some import methods of the class.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.index('joint name') returns the index of the joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.index(' wrist_3_joint')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.names is a container (~list) that contains all the joint names"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
128
129
    "for i, n in enumerate(robot.model.names):\n",
    "    print(i, n)"
Nicolas Mansard's avatar
Nicolas Mansard committed
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.frames contains all the import frames attached to the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for f in robot.model.frames:\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
146
    "    print(f.name, 'attached to joint #', f.parent)"
Nicolas Mansard's avatar
Nicolas Mansard committed
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
162
    "robot.placement(robot.q0, 6)  # Placement of the end effector."
Nicolas Mansard's avatar
Nicolas Mansard committed
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dimension of the configuration space (i.e. the number of joints) is given in:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "NQ = robot.model.nq\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
179
    "NV = robot.model.nv  # for this simple robot, NV == NQ"
Nicolas Mansard's avatar
Nicolas Mansard committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Display simple geometries\n",
    "The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
196
197
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors  # noqa: E402\n",
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration *q* can be displayed in the viewer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([-1., -1.5, 2.1, -.5, -.5, 0])\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
225
    "viz.display(q)"
Nicolas Mansard's avatar
Nicolas Mansard committed
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Other geometries (cubes, spheres, etc) can be displayed as well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
241
    "# %load tp2/generated/simple_pick_and_place_2\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
242
243
244
    "\n",
    "# Add a red box in the viewer\n",
    "ballID = \"world/ball\"\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
245
    "viz.addSphere(ballID, 0.1, colors.red)\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
246
247
248
249
    "\n",
    "# Place the ball at the position ( 0.5, 0.1, 0.2 )\n",
    "# The viewer expect position and rotation, apppend the identity quaternion\n",
    "q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
250
    "viz.applyConfiguration(ballID, q_ball)"
Nicolas Mansard's avatar
Nicolas Mansard committed
251
252
253
254
255
256
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
257
    "# Forward (direct) geometry\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
258
259
260
    "\n",
    "First, let's do some forward geometry, i.e. use Pinocchio to compute where is the end effector knowning the robot configuration.\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
261
    "# Simple pick ...\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
262
263
264
265
266
267
268
269
270
271
272
    "\n",
    "Say we have a target at position [.5,.1,.2] and we would like the robot to grasp it.\n",
    "First decide (by any way you want, e.g. trial and error) the configuration of the robot so that the end effector touches the ball.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
273
    "# %load tp2/generated/simple_pick_and_place_3\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
274
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
275
    "q0 = np.zeros(NQ)  # set the correct values here\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
276
277
278
279
280
281
    "q0[0] = 0.5\n",
    "q0[1] = 0.\n",
    "q0[2] = -1.5\n",
    "q0[3] = 0.\n",
    "q0[4] = 0.\n",
    "q0[5] = 0.\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
282
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
283
284
285
286
287
288
289
290
291
292
    "viz.display(q0)\n",
    "\n",
    "# Take care to explicitely mention copy when you want a copy of array.\n",
    "q = q0.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
293
    "# ... and simple place"
Nicolas Mansard's avatar
Nicolas Mansard committed
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the reference position you built, the end effector placement can be obtained by calling "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
309
    "robot.placement(q, 6).translation"
Nicolas Mansard's avatar
Nicolas Mansard committed
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Only the translation part of the placement has been selected. The rotation is free.\n",
    "\n",
    "Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
327
    "time.sleep(.1)  # in second"
Nicolas Mansard's avatar
Nicolas Mansard committed
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At each instant of your loop, recompute the position of the ball and display it so that it always \"sticks\" to the robot end effector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
343
344
345
346
    "# TODO ####################################################\n",
    "# Replace here with your initial configuration\n",
    "q = q0 = np.random.rand(NQ) * 6 - 3\n",
    "# TODO ####################################################\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
347
348
349
350
351
352
353
354
355
    "\n",
    "# Compute initial translation between effector and box.\n",
    "# Translation of end-eff wrt world at initial configuration\n",
    "o_eff = robot.placement(q, 6).translation\n",
    "# Translation of box wrt world\n",
    "o_ball = q_ball[:3]\n",
    "eff_ball = o_ball - o_eff\n",
    "\n",
    "for i in range(100):\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
356
    "    # Replace here by your choice of computing q(t)\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
357
    "    q += np.random.rand(6) * 2e-1 - 1e-1\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
358
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
359
360
    "    # TODO ####################################################\n",
    "    # Replace here by your computation of the new box position\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
361
    "    o_ball = np.array([0., 0., 0.])\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
362
363
    "    # /TODO ###################################################\n",
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
364
365
    "    # Display the new robot and box configurations.\n",
    "    # The viewer expect a placement (position-rotation).\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
366
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
367
    "    viz.display(q)\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
368
    "    time.sleep(0.1)"
Nicolas Mansard's avatar
Nicolas Mansard committed
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution is below, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
384
    "%do_not_load tp2/generated/simple_pick_and_place_4"
Nicolas Mansard's avatar
Nicolas Mansard committed
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pick and place in 3D\n",
    "\n",
    "Say now that the object is a rectangle and not a sphere. Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
402
    "# %load tp2/generated/simple_pick_and_place_5\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
403
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
404
405
    "# Add a red box in the viewer\n",
    "boxID = \"world/box\"\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
406
407
    "viz.delete(ballID)\n",
    "viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
408
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
409
    "# Place the box at the position (0.5, 0.1, 0.2)\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
410
    "q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
411
    "viz.applyConfiguration('world/box', q_box)"
Nicolas Mansard's avatar
Nicolas Mansard committed
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration with the arm nicely attached to the box is:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
427
    "# %load tp2/generated/simple_pick_and_place_6\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
428
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
429
430
431
432
433
    "q0 = np.zeros(NQ)\n",
    "q0[0] = -0.375\n",
    "q0[1] = -1.2\n",
    "q0[2] = 1.71\n",
    "q0[3] = -q0[1] - q0[2]\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
434
    "q0[4] = q0[0]\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
    "\n",
    "viz.display(q0)\n",
    "q = q0.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Redo the same question as before, but now also choosing the orientation of the box. For that, at each robot configuration in your for-loop, compute the box placement wrt the world (let's denote it by oMbox) and display both the box and the robot configuration in the view."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
453
    "%do_not_load tp2/generated/simple_pick_and_place_7"
Nicolas Mansard's avatar
Nicolas Mansard committed
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inverse geometry\n",
    "\n",
    "We only yet computed the forward geometry, i.e. from configurations to end-effector placement. Let's to the inverse map not.\n",
    "\n",
    "### Inverse geometry in 3D\n",
    "\n",
    "Let's now first control the position (i.e. translation only) of the end effector of a manipulator robot to a given position. For this first part, we will use the fixed serial-chain robot model.\n",
    "\n",
    "Recall first that the position (3D) of the joint with index \"i=6\" at position \"q\" can be access by the following two lines of code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
477
    "robot.placement(q, 6).translation"
Nicolas Mansard's avatar
Nicolas Mansard committed
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p. For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effetor and the target."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
493
    "%do_not_load tp2/generated/invgeom3d_1"
Nicolas Mansard's avatar
Nicolas Mansard committed
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 6D\n",
    "6D means: translation and rotation. Change the previous cost function for a cost measuring the difference between the current placement root.placement(q,6) and a reference placement oMdes. \n",
    "For that, you can use the SE(3) log function to score the distance between two placements. The log returns a 6D velocity, represented by a class Motion, that must be transformed to a vector of R^6 from which you can take the norm.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.log(pin.SE3.Identity()).vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
520
    "# %load tp2/generated/invgeom6d_1\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
521
522
523
524
    "\n",
    "# --- Add box to represent target\n",
    "viz.addBox(\"world/box\", [.05, .1, .2], [1., .2, .2, .5])\n",
    "viz.addBox(\"world/blue\", [.05, .1, .2], [.2, .2, 1., .5])\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
525
526
527
528
529
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
530
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
531
532
533
534
535
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    M = robot.placement(q, 6)\n",
    "    return norm(pin.log(M.inverse() * Mtarget).vector)\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
536
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
537
    "def callback(q):\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
538
    "    viz.applyConfiguration('world/box', Mtarget)\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
539
540
541
542
    "    viz.applyConfiguration('world/blue', robot.placement(q, 6))\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-1)\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
543
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
544
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
545
546
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n",
    "\n",
Guilhem Saurel's avatar
Guilhem Saurel committed
547
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
Nicolas Mansard's avatar
Nicolas Mansard committed
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Optimizing in the quaternion space\n",
    "\n",
    "Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robex.load('solo12')\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
    "# %load tp2/generated/floating_1\n",
    "\n",
    "robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]\n",
    "\n",
    "# --- Add box to represent target\n",
    "colors = ['red', 'blue', 'green', 'magenta']\n",
    "for color in colors:\n",
    "    viz.addSphere(\"world/%s\" % color, .05, color)\n",
    "    viz.addSphere(\"world/%s_des\" % color, .05, color)\n",
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
    "targets = [\n",
    "    np.array([-0.7, -0.2, 1.2]),\n",
    "    np.array([-0.3, 0.5, 0.8]),\n",
    "    np.array([0.3, 0.1, -0.1]),\n",
    "    np.array([0.9, 0.9, 0.5])\n",
    "]\n",
    "for i in range(4):\n",
    "    targets[i][2] += 1\n",
    "\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    cost = 0.\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n",
    "        cost += norm(p_i - targets[i])**2\n",
    "    return cost\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.applyConfiguration('world/box', Mtarget)\n",
    "\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i])\n",
    "        viz.applyConfiguration('world/%s' % colors[i], p_i)\n",
    "        viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])\n",
    "\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-1)\n",
    "\n",
    "\n",
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)"
Nicolas Mansard's avatar
Nicolas Mansard committed
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration of parallel robots\n",
    "A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n",
    "\n",
    "The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
658
659
    "import utils.load_ur5_parallel as robex2  # noqa: E402\n",
    "\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
    "robot = robex2.load()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "[w, h, d] = [0.5, 0.5, 0.005]\n",
    "color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n",
    "viz.addBox('world/robot0/toolplate', [w, h, d], color)\n",
    "Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n",
    "viz.applyConfiguration('world/robot0/toolplate', Mtool)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
710
711
    "effIdxs = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n",
    "robot.framePlacement(robot.q0, effIdxs[0])"
Nicolas Mansard's avatar
Nicolas Mansard committed
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n",
    "\n",
    "Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
Guilhem Saurel's avatar
Guilhem Saurel committed
730
    "quat = pin.Quaternion(0.7, 0.2, 0.2, 0.6).normalized()\n",
Nicolas Mansard's avatar
Nicolas Mansard committed
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
    "print(quat.matrix())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
Guilhem Saurel's avatar
Guilhem Saurel committed
758
   "version": "3.9.6"
Nicolas Mansard's avatar
Nicolas Mansard committed
759
760
761
762
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
Guilhem Saurel's avatar
Guilhem Saurel committed
763
}