-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpick.html
1606 lines (1326 loc) · 86.3 KB
/
pick.html
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
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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
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
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
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
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
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
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
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
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
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
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
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
<!DOCTYPE html>
<html>
<head>
<title>Ch. 3 - Basic Pick and Place</title>
<meta name="Ch. 3 - Basic Pick and Place" content="text/html; charset=utf-8;" />
<link rel="canonical" href="http://manipulation.csail.mit.edu/pick.html" />
<script src="https://hypothes.is/embed.js" async></script>
<script type="text/javascript" src="chapters.js"></script>
<script type="text/javascript" src="htmlbook/book.js"></script>
<script src="htmlbook/mathjax-config.js" defer></script>
<script type="text/javascript" id="MathJax-script" defer
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-chtml.js">
</script>
<script>window.MathJax || document.write('<script type="text/javascript" src="htmlbook/MathJax/es5/tex-chtml.js" defer><\/script>')</script>
<link rel="stylesheet" href="htmlbook/highlight/styles/default.css">
<script src="htmlbook/highlight/highlight.pack.js"></script> <!-- http://highlightjs.readthedocs.io/en/latest/css-classes-reference.html#language-names-and-aliases -->
<script>hljs.initHighlightingOnLoad();</script>
<link rel="stylesheet" type="text/css" href="htmlbook/book.css" />
</head>
<body onload="loadChapter('manipulation');">
<div data-type="titlepage" pdf="no">
<header>
<h1><a href="index.html" style="text-decoration:none;">Robotic Manipulation</a></h1>
<p data-type="subtitle">Perception, Planning, and Control</p>
<p style="font-size: 18px;"><a href="http://people.csail.mit.edu/russt/">Russ Tedrake</a></p>
<p style="font-size: 14px; text-align: right;">
© Russ Tedrake, 2020-2022<br/>
Last modified <span id="last_modified"></span>.</br>
<script>
var d = new Date(document.lastModified);
document.getElementById("last_modified").innerHTML = d.getFullYear() + "-" + (d.getMonth()+1) + "-" + d.getDate();</script>
<a href="misc.html">How to cite these notes, use annotations, and give feedback.</a><br/>
</p>
</header>
</div>
<p pdf="no"><b>Note:</b> These are working notes used for <a
href="http://manipulation.csail.mit.edu/Fall2022/">a course being taught
at MIT</a>. They will be updated throughout the Fall 2022 semester. <!-- <a
href="https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg">Lecture videos are available on YouTube</a>.--></p>
<table style="width:100%;" pdf="no"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=robot.html>Previous Chapter</a></td>
<td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
<td style="width:33%;text-align:right;"><a class="next_chapter" href=pose.html>Next Chapter</a></td>
</tr></table>
<script type="text/javascript">document.write(notebook_header('pick'))
</script>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 2"><h1>Basic Pick and Place</h1>
<figure>
<img src="figures/pick.png" style="width:70%"/>
<figcaption>Your challenge: command the robot to pick up the brick and place
it in a desired position/orientation.</figcaption>
<todo>Update this image once we get a better schunk model.</todo>
</figure>
<p>The stage is set. You have your robot. I have a little red foam brick.
I'm going to put it on the table in front of your robot, and your goal is to
move it to a desired position/orientation on the table. I want to defer the
<i>perception</i> problem for one more chapter, and will let you assume that
you have access to a perfect measurement of the current position/orientation
of the brick. Even without perception, completing this task requires us to
build up a basic toolkit for geometry and kinematics; it's a natural place to
start.</p>
<p>First, we will establish some terminology and notation for kinematics. This
is one area where careful notation can yield dividends, and sloppy notation
will inevitably lead to confusion and bugs. The Drake developers have gone to
great length to establish and document a consistent <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html">
multibody notation</a>, which we call "Monogram Notation". The documentation
even includes some of the motivation/philosophy behind that notation. I'll use
the monogram notation throughout this text.</p>
<p>If you'd like a more extensive background on kinematics than what I provide
here, my favorite reference is still <elib>Craig05</elib>. For free online
resources, Chapters 2 and 3 of the 1994 book by Murray et al. (now free
online)<elib>Murray94</elib> are also excellent, as are the first seven
chapters of <a
href="http://hades.mech.northwestern.edu/index.php/Modern_Robotics">Modern
Robotics</a> by Lynch and Park<elib>Lynch17</elib> (they also have excellent
accompanying videos). Unfortunately, with three different references you'll
get three (slightly) different notations; ours is most similar to
<elib>Craig05</elib>. The monogram notation is developed in some detail in
<elib>Mitiguy17</elib>.</p>
<p>Please don't get overwhelmed by how much background material there is to
know! I am personally of the opinion that a clear understanding of just a few
basic ideas should make you very effective here. The details will come
later, if you need them.</p>
<section id="monogram"><h1>Monogram Notation</h1>
<p><i>The following concepts are disarmingly subtle. I've seen incredibly
smart people assume they knew them and then perpetually stumble over
notation. I did it for years myself. Take a minute to read this
carefully!</i></p>
<p>Perhaps the most fundamental concept in geometry is the concept of a
point. Points occupy a position in space, and they can have names, e.g.
point $A$, $C$, or more descriptive names like $B_{cm}$ for the center of
mass of body $B$. We'll denote the position of the point by using a position
vector $p^A$; that's $p$ for position, and not for point, because other
geometric quantities can also have a position.</p>
<p>But let's be more careful. Position is actually a relative quantity.
Really, we should only ever write the position of two points relative to
each other. We'll use e.g. $^Ap^C$ to denote the position of $C$ measured
from $A$. The left superscript looks mighty strange, but we'll see that it
pays off once we start transforming points.</p>
<p>Every time we describe the (relative) position as a vector of numbers,
we need to be explicit about the frame we are using, specifically the
"expressed-in" frame. All of our frames are defined by orthogonal unit
vectors that follow the "right-hand rule". We'll give a frame a name, too,
like $F$. If I want to write the position of point $C$ measured from point
$A$, expressed in frame $F$, I will write $^Ap^C_F$. If I ever want to get
just a single component of that vector, e.g. the $x$ component, then I'll
use $^Ap^C_{F_x}$. In some sense, the "expressed-in" frame is an
implementation detail; it is only required once we want to represent the
multibody quantity as a vector (e.g. in the computer).</p>
<p>That is seriously heavy notation. I don't love it myself, but it's
the most durable I've got, and we'll have shorthand for when the context is
clear.</p>
<p><img style="height:90px;float:left;margin-right:10px"
src="figures/frames.png"/> There are a few very special frames.
We use $W$ to denote the <i>world frame</i>. We think about the world frame
in Drake using vehicle coordinates (positive $x$ to the front, positive $y$
to the <i>left</i>, and positive $z$ is up). The other particularly special
frames are the <i>body frames</i>: every body in the multibody system has a
unique frame attached to it. We'll typically use $B_i$ to denote the frame
for body $i$.</p>
<p>Frames have a position, too -- it coincides with the frame origin. So it
is perfectly valid to write $^Wp^A_W$ to denote the position of point $A$
measured from the origin of the world frame, expressed in the world frame.
Here is where the shorthand comes in. If the position of a quantity is
measured from a frame, and expressed in the same frame, then we can safely
omit the subscript. $^Fp^A \equiv {^Fp^A_F}$. Furthermore,
if the "measured from" field is omitted, then we assume that the point is
measured from $W$, so $p^A \equiv {}^Wp^A_W$.</p>
<figure>
<img style="width:60%" src="figures/notation.svg"></img>
<!-- Note: I made this in slides.com, exported to pdf, cropped in acrobat,
then converted with pdf2svg. -->
</figure>
<p>Frames also have an orientation. We'll use $R$ to denote a
<i>rotation</i>, and follow the same notation, writing $^BR^A$ to denote
the rotation of frame $A$ measured from frame $B$. Unlike vectors, pure
rotations do not have an additional "expressed in" frame.</p>
<p>A frame $F$ can be specified completely by a position and rotation
measured from another frame. Taken together, we call the position and
rotation a <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html">
<i>spatial pose</i></a>, or just <i>pose</i>. A <i>spatial transform</i>,
or just <i>transform</i>, is the "verb form" of pose. In Drake we use <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1math_1_1_rigid_transform.html"><code>RigidTransform</code></a>
to represent a pose/transform, and denote it with the letter $X$. $^BX^A$
is the pose of frame $A$ measured from frame $B$. When we talk about the
pose of an object $O$, without mentioning a reference frame explicitly, we
mean $^WX^O$ where $O$ is the body frame of the object. We do not use the
"expressed in" frame subscript for pose; we always want the pose expressed
in the reference frame.</p>
<p>The Drake <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html">
documentation</a> also discusses how to use this notation in code. In
short, $^Bp^A_C$ is written <code>p_BA_C</code>, ${}^BR^A$ as
<code>R_BA</code>, and ${}^BX^A$ as <code>X_BA</code>. It works, I
promise.</p>
</section>
<section><h1>Pick and place via spatial transforms</h1>
<p>Now that we have the notation, we can formulate our approach to the basic
pick and place problem. Let us call our object, $O$, and our gripper, $G$.
Our idealized perception sensor tells us $^WX^O$. Let's create a frame
$O_d$ to describe the "desired" pose of the object, $^WX^{O_d}$.
So pick and place manipulation is simply trying to make $X^O = X^{O_d}$.</p>
<todo>Add a figure here (after Terry's PR lands).</todo>
<div>To accomplish this, we will assume that the object doesn't move
relative to the world ($^WX^O$ is constant) when the gripper is open, and
the object doesn't move relative to the gripper ($^GX^O$ is constant) when
the gripper is closed. Then we can:
<ul style="margin:0"><li>move the gripper in the world, $X^G$, to an
appropriate pose measured from the object: $^OX^{G_{grasp}}$.</li>
<li>close the gripper.</li>
<li>move the gripper+object to the desired pose, $X^O = X^{O_d}$.</li>
<li>open the gripper, and retract the hand.</li>
</ul>
There is just one more important detail: to approach the object without
colliding with it, we will insert a "pregrasp pose", $^OX^{G_{pregrasp}}$,
above the object as an intermediate step. We'll use the same transform to
retract away from the object when we set it down.
</div>
<p>Clearly, programming this strategy requires good tools for working with
these transforms, and for relating the pose of the gripper to the joint
angles of the robot.</p>
</section>
<section id="spatial_algebra"><h1>Spatial Algebra</h1>
<p>Here is where we start to see the pay-off from our heavy notation, as we
define the rules of converting positions, rotations, poses, etc. between
different frames. Without the notation, this invariably involves
me with my right hand in the air making the "right-hand rule", and my head
twisting around in space. With the notation, it's a simple matter of lining
up the symbols properly, and we're more likely to get the right answer!</p>
<div>
Here are the basic rules of algebra for our spatial quantities:
<ul>
<li>Positions expressed in the same frame can be added when their
reference and target symbols match: \begin{equation}{}^Ap^B_F + {}^Bp^C_F
= {}^Ap^C_F.\end{equation} Addition is commutative, and the additive
inverse is well defined: \begin{equation}{}^Ap^B_F = -
{}^Bp^A_F.\end{equation} Those should be pretty intuitive; make sure you
confirm them for yourself.</li>
<li>Multiplication by a rotation can be used to change the "expressed in"
frame: \begin{equation}{}^Ap^B_G = {}^GR^F {}^Ap^B_F.\end{equation} You
might be surprised that a rotation alone is enough to change the
expressed-in frame, but it's true. The position of the expressed-in frame
does <i>not</i> affect the relative position between two points.</li>
<li>Rotations can be multiplied when their reference and target symbols
match: \begin{equation}{}^AR^B \: {}^BR^C = {}^AR^C.\end{equation} The
inverse operation is also simply defined:
\begin{equation}\left[{}^AR^B\right]^{-1} = {}^BR^A.\end{equation} When
the rotation is represented as a rotation matrix, this is literally the
matrix inverse, and since rotation matrices are orthonormal, we also have
$R^{-1}=R^T.$</li>
<li>Transforms bundle this up into a single, convenient notation when
positions are measured from a frame (and the same frame they are expressed
in): \begin{equation}{}^Gp^A = {}^GX^F {}^Fp^A = {}^Gp^F + {}^Fp^A_G
= {}^Gp^F + {}^GR^F {}^Fp^A.\end{equation}</li>
<li>Transforms compose: \begin{equation}{}^AX^B {}^BX^C =
{}^AX^C,\end{equation} and have an inverse
\begin{equation}\left[{}^AX^B\right]^{-1} = {}^BX^A.\end{equation} Please
note that for transforms, we generally do
<i>not</i> have that $X^{-1}$ is $X^T,$ though it still has a simple form.
</li>
</ul></div>
<p>In practice, transforms are implemented using <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html">homogenous
coordinates</a>, but for now I'm happy to leave that as an implementation
detail.</p>
<example><h1>From camera frame to world frame</h1>
<todo>Add a figure here.</todo>
<p>Imagine that I have a depth camera mounted in a fixed pose in my
workspace. Let's call the camera frame $C$ and denote its pose in the
world with ${}^WX^C$.</p>
<p>A depth camera returns points in the camera frame. Therefore,
we'll write this position of point $P_i$ with ${}^Cp^{P_i}$. If we want
to convert the point into the world frame, we simply have $$p^{P_i}
= X^C {}^Cp^{P_i}.$$</p>
<p>This is a work-horse operation for us. We often aim to merge points
from multiple cameras (typically in the world frame), and always need to
somehow relate the frames of the camera with the frames of the robot. The
inverse transform, ${}^CX^W$, which projects world coordinates into the
camera frame, is often called the camera "extrinsics".</p>
</example>
<subsection id="3D_rotation"><h1>Representations for 3D rotation</h1>
<p>In the spatial algebra above, I've written the rules for rotations
using an abstract notion of rotation. But in order to implement this
algebra in code, we need to decide how we are going to represent those
representations with a small number of real values. There are <a
href="https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions">many
possible representations of 3D rotations</a>, they are each good for
different operations, and unfortunately, there is no one representation
to rule them all. (This is one of the many reasons why everything is
better in 2D!) Common representations include <ul><li><a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1math_1_1_rotation_matrix.html">3$\times$3
rotation matrices</a>,</li>
<li><a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1math_1_1_roll_pitch_yaw.html">Euler
angles (e.g. roll-pitch-yaw)</a>,</li><li><a
href="https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html">axis
angle</a> (aka "exponential coordinates"), and </li><li><a
href="https://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html">unit
quaternions</a>.</li></ul> In Drake, we provide all of these
representations, and make it easy to convert back and forth between
them.</p>
<p>A 3$\times$3 <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html#orientation_discussion">rotation
matrix</a> is an orthonormal matrix with columns formed by the $x-$,
$y-$, and $z-$axes. Specifically, the first column of the transform
${}^GR^F$ has the $x-$axis of frame $F$ expressed in $G$ in the first
column, etc.</p>
<p><a href="https://en.wikipedia.org/wiki/Euler_angles">Euler angles</a>
specify a 3D rotation by a series of rotations around the $x$, $y$, and
$z$ axes. The order of these rotations matters; and many combinations can be used to describe any 3D rotation. This is why we use
<code>RollPitchYaw</code> in the code (preferring it over the more
general term "Euler angle") and <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1math_1_1_roll_pitch_yaw.html">document
it carefully</a>. Roll is a rotation around the $x$-axis, pitch is a
rotation around the $y$-axis, and yaw is a rotation around the $z$-axis;
this is also known as "extrinsic X-Y-Z" Euler angles.</p>
<p>Axis angles describe a 3D rotation by a scalar rotation around an
arbitrary vector axis using three numbers: the direction of the vector is
the axis, and the magnitude of the vector is the angle. You can think of
unit quaternions as a form of axis angles that have been carefully
normalized to be unit length and have magical properties. My favorite
careful description of quaternions is probably chapter 1 of
<elib>Stillwell08</elib>.</p>
<p>Why all of the representations? Shouldn't "roll-pitch-yaw" be enough?
Unfortunately, no. The limitation is perhaps most easily seen by looking
at the coordinate changes from roll-pitch-yaw to/from a rotation matrix.
Any roll-pitch-yaw can be converted to a rotation matrix, but the inverse
map has a singularity. In particular, when the pitch angle is
$\frac{\pi}{2}$, then roll and yaw are now indistinguishable. This is
described very nicely, along with its physical manifestation in "gimbal
lock" in <a
href="https://robotacademy.net.au/lesson/singularity-in-3d-rotation-angle-sequences/">this
video</a>. Similarly, the direction of the vector in the axis-angle
representation in not uniquely defined when the rotation angle is zero.
These singularities become problematic when we start taking derivatives
of rotations, for instance when we write the equations of motion. It is
now well understood <elib>Stillwell08</elib> that it requires at least
four numbers to properly represent the group of 3D rotations; the unit
quaternion is the most common four-element representation.
</p>
</subsection>
</section>
<section id="kinematics"><h1>Forward kinematics</h1>
<p>The spatial algebra gets us pretty close to what we need for our pick and
place algorithm. But remember that the interface we have with the robot
reports measured joint positions, and expects commands in the form of joint
positions. So our remaining task is to convert between joint angles and
cartesian frames. We'll do this in steps, the first step is to go from
joint positions to cartesian frames: this is known as <i>forward
kinematics</i>.
<p>Throughout this text, we will refer to the joint positions of the robot
(also known as "configuration" of the robot) using a vector $q$. If the
configuration of the scene includes objects in the environment as well as
the robot, we would use $q$ for the entire configuration vector, and use
e.g. $q_{robot}$ for the subset of the vector corresponding to the robot's
joint positions. Therefore, the goal of forward kinematics is to produce a
map: \begin{equation}X^G = f_{kin}^G(q).\end{equation} Moreover, we'd like
to have forward kinematics available for any frame we have defined in the
scene. Our spatial notation and spatial algebra makes this computation
relatively straight-forward.</p>
<subsection id="kinematic_tree"><h1>The kinematic tree</h1>
<p>In order to facilitate kinematics and related multibody computations,
the <code>MultibodyPlant</code> organizes all of the bodies in the world
into a tree topology. Every body (except the world body) has a parent,
which it is connected to via either a <code>Joint</code> or a "floating
base".</p>
<example><h1>Inspecting the kinematic tree</h1>
<p>Drake provides some visualization support for inspecting the
kinematic tree data structure. The kinematic tree for an iiwa is more
of a vine than a tree (it's a serial manipulator), but the tree for the
dexterous hands are more interesting. I've added our brick to the
example, too, so that you can see that a "free" body is just another
branch off the world root node.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="kinematic_tree"))</script>
<todo>Insert topology visualization here (once it is better)</todo>
</example>
<p>Every <code>Joint</code> and "floating base" has some number of
position variables associated with it -- a subset of the configuration
vector $q$ -- and knows how to compute the configuration dependent
transform across the joint from the child joint frame $J_C$ to the parent
joint frame $J_P$: ${}^{J_P}X^{J_C}(q)$. Additionally, the kinematic tree
defines the (fixed) transforms from the joint frame to the child body
frame, ${}^CX^{J_C}$, and from the joint frame to the parent frame,
${}^PX^{J_P}$. Altogether, we can compute the configuration transform
between any one body and its parent, $${}^PX^C(q) = {}^PX^{J_P}
{}^{J_P}X^{J_C}(q) {}^{J_C}X^C.$$</p>
<todo>
Examples or links to specifying the kinematic tree in URDF, SDF, etc...
</todo>
<p>You might be tempted to think that every time you add a joint to the
<code>MultibodyPlant</code>, you are adding a degree of freedom. But it
actually works the other way around. Every time you add a <i>body</i>
to the plant, you are adding many degrees of freedom. But you can then
add joints to <i>remove</i> those degrees of freedom; joints are
constraints. "Welding" the robot's base to the world frame removes all of
the floating degrees of freedom of the base. Adding a rotational joint
between a child body and a parent body removes all but one degree of
freedom, etc.</p>
</subsection>
<subsection><h1>Forward kinematics for pick and place</h1>
<p>In order to compute the pose of the gripper in the world, $X^G$, we
simply query the parent of the gripper frame in the kinematic tree, and
recursively compose the transforms until we get to the world frame.</p>
<figure id="kinematic_frames">
<img style="width:40%;margin-right:40px"
src="figures/kinematic_frames_iiwa.png"/><img style="width:40%"
src="figures/kinematic_frames_gripper.png"/>
<figcaption>Kinematic frames on the iiwa (left) and the WSG (right). For
each frame, the <span style="color:red">positive $x$ axis is in
red</span>, the <span style="color:green">positive $y$ axis is in
green</span>, and the <span style="color:blue">positive $z$ axis is in
blue</span>. It's (hopefully) easy to remember: XYZ $\Leftrightarrow$
RGB.</figcaption>
</figure>
<example><h1>Forward kinematics for the gripper frame</h1>
<p>Let's evaluate the pose of the gripper in the world frame: $X^G$.
We know that it will be a function of configuration of the robot, which
is just a part of the total state of the <code>MultibodyPlant</code>
(and so is stored in the <code>Context</code>). The following example
shows you how it works.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="forward_kinematics"))</script>
<p>The key lines are
<code class="language-python"><pre>
gripper = plant.GetBodyByName("body", wsg)
pose = plant.EvalBodyPoseInWorld(context, gripper)</pre></code>
Behind the scenes, the <code>MultibodyPlant</code> is doing all of the
spatial algebra we described above to return the pose (and also some
clever caching because you can reuse much of the computation when you
want to evaluate the pose of another frame on the same robot).
</p>
</example>
<example id="floating-base"><h1>Forward kinematics of "floating-base" objects</h1>
<p>Consider the special case of having a <code>MultibodyPlant</code>
with exactly one body added, and no joints. The kinematic tree is
simply the world frame, the body frame, and they are connected by the
"floating base". What does the forward kinematics function: $$X^B =
f_{kin}^B(q),$$ look like in that case? If $q$ is already representing
the floating-base configuration, is $f^B_{kin}$ just the identity
function?</p>
<p>This gets into the subtle points of how we represent transforms, and
<a href="#3D_rotation">how we represent 3D rotations</a> in particular. Although we use rotation matrices in our <code>RigidTransform</code> class, in order to make the spatial algebra efficient, we actually use unit quaternions in the <code>Context</code> in order to have a more compact vector representation.</p>
<p>As a result, for this example, the software implementation of the
function $f_{kin}^B$ is precisely the function that converts the
position $\times$ unit quaternion representation into the position
$\times$ rotation matrix representation.</p>
</example>
</subsection>
</section>
<section id="jacobian"><h1>Differential kinematics (Jacobians)</h1>
<p>The forward kinematics machinery gives us the ability to compute the pose
of the gripper and the pose of the object, both in the world frame. But
if our goal is to <i>move</i> the gripper to the object, then we should
understand how changes in the joint angles relate to changes in the gripper
pose. This is traditionally referred to as "differential kinematics".</p>
<p>At first blush, this is straightforward. The change in pose is related
to a change in joint positions by the (partial) derivative of the forward
kinematics: \begin{equation}dX^B = \pd{f_{kin}^B(q)}{q} dq = J^B(q)dq.
\label{eq:jacobian}\end{equation} Partial derivatives of a function are
referred to as "Jacobians" in many fields; in robotics it's rare to refer to
derivatives of the kinematics as anything else.</p>
<div>All of the subtlety, again, comes in because of the multiple
representations that we have for 3D rotations (rotation matrix, unit
quaternions, ...). While there is no one best representation for 3D
rotations, it <i>is</i> possible to have one canonical representation for
<i>differential</i> rotations. Without any concern for singularities nor
loss of generality, we can represent the rate of change in pose using a
six-component vector for <a
href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__vectors.html"><i>spatial
velocity</i></a>: \begin{equation}{}^AV^B_C = \begin{bmatrix}
{}^A\omega^B_C \\ {}^A\text{v}^B_C \end{bmatrix}.\end{equation} ${}^AV^B_C$
is the spatial velocity (also known as a "twist") of frame $B$ measured in
frame $A$ expressed in frame $C$, ${}^A\omega^B_C \in \Re^3$ is the
<i>angular velocity</i> (of frame $B$ measured in $A$ expressed in frame
$C$), and ${}^A\text{v}^B_C \in \Re^3$ is the <i>translational velocity</i>
(along with the same shorthands as for positions). The angular velocity is
a 3D vector (with $w_x$, $w_y$, $w_z$ components); the magnitude of this
vector represents the angular speed and the direction represents the
(instantaneous) axis of rotation. It's tempting to think of it as the time
derivatives of roll, pitch, and yaw, but that's not true; it can easily be
converted into that representation through a <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#a7bb24fed19f16a76450eece1857b92dc">nonlinear
change of coordinates</a>. Spatial velocities fit nicely into our spatial
algebra:<ul>
<li>Angular velocities add (when the frames match): \begin{equation}
{}^A\omega^B_F + {}^B\omega^C_F = {}^A\omega^C_F,\end{equation} (this <a
href="#angular_velocity_addition">deserves to be verified</a>) and have
the additive inverse, ${}^A\omega^C_F = - {}^C\omega^A_F,$.</li>
<li>Rotations can be used to change between the "expressed-in"
frames: \begin{equation} {}^A\text{v}^B_G = {}^GR^F {}^A\text{v}^B_F,
\qquad {}^A\omega^B_G = {}^GR^F {}^A\omega^B_F.\end{equation}
</li>
<li>Translational velocities have the additive inverse,
${}^A\text{v}^C_F = - {}^C\text{v}^A_F,$ and compose across frames
with: \begin{equation}{}^A\text{v}^C_F = {}^A\text{v}^B_F +
{}^B\text{v}^C_F + {}^A\omega^B_F \times {}^Bp^C_F.\end{equation}
<details><summary>This can be derived in a few steps (click the triangle
to expand)</summary><p>Differentiating $${}^Ap^C = {}^AX^B {}^Bp^C =
{}^Ap^B + {}^AR^B {}^Bp^C,$$ yields \begin{align} {}^A\text{v}^C =&
{}^A\text{v}^B + {}^A\dot{R}^B {}^Bp^C + {}^AR^B {}^B\text{v}^C \nonumber
\\ =& {}^A\text{v}^B_A + {}^A\dot{R}^B {}^BR^A {}^Bp^C_A +
{}^B\text{v}^C_A.\end{align} Allow me to write $\dot{R}R^{-1}$ for
${}^A\dot{R}^B {}^BR^A$ (dropping the frames for a moment). It turns out
that $\dot{R}R^{-1}$, is always a skew-symmetric matrix. To see this,
differentiate $RR^T = I$ to get $$\dot{R}R^T + R\dot{R}^T = 0 \Rightarrow
\dot{R}R^T = - R\dot{R}^T \Rightarrow \dot{R} R^T = - (\dot{R} R^T)^T,$$
which is the definition of a skew-symmetric matrix. Any 3$\times$3
skew-symmetric matrix can be parameterized by three numbers (we'll use
the three-element vector $\omega$), and can be written as a cross
product, so $\dot{R}R^Tp = \omega \times p.$</p> <p>Multiply the right
and left sides by ${}^FR^A$ to change the expressed-in frame, and we have
our result.</p></details></li>
</ul>
</div>
<details><summary>If you're familiar with "screw theory" (as used in, e.g.
<elib>Murray94</elib> and <elib>Lynch17</elib>), click the triangle to
see how those conventions are related.</summary>
<ul><p>Screw theory (as used in, e.g.
<elib>Murray94</elib> and <elib>Lynch17</elib>) often uses a particular
form of our spatial velocity referred to as "spatial velocity in the space
frame" / "spatial twists" that can be useful in a number of computations.
This quantity is ${}^AV^{B_A}$, where $B_A$ is the frame rigidly attached
on body $B$ (so ${}^BV^{B_A} = 0$) that is instantaneously in the same pose
as $A$ (so ${}^Ap^{B_A} = 0$, ${}^AR^{B_A} = I$) <elib
part="3.3.2">Lynch17</elib>. These conditions reduce to
\begin{equation}{}^AV^{B_A} = \begin{bmatrix} {}^A\omega^{B_A} \\
{}^A\text{v}^{B_A} \end{bmatrix} = \begin{bmatrix} {}^A\omega^{B_A} +
{}^{B_A}\omega^B_{A} \\ {}^A\text{v}^B + {}^B\text{v}^{B_A}_A +
{}^A\omega^B \times {}^Bp^{B_A}_A \end{bmatrix} = \begin{bmatrix} {}^A
\omega^{B} \\ {}^A\text{v}^B - {}^A\omega^B \times {}^Ap^{B}
\end{bmatrix}.\end{equation}
</p></ul> </details>
<!--
<p>Why is it that we can get away with just three components for spatial
velocity, but not for rotations? In short, this is because a three
component vector has both a direction (which we use to denote the axis) and
a magnitude. Using the magnitude to denote an angle is degenerate because
our representation should be periodic in $2\pi$, but using them for angular
velocities doesn't have this problem. Using sine of the angle resolves it,
but we also need to store the cosine; that's <a
href="https://en.wikipedia.org/w/index.php?title=Quaternions_and_spatial_rotation§ion=14#The_hypersphere_of_rotations">precisely
what happens</a> in the unit quaternions!</p> -->
<p>There is one more velocity to be aware of: I'll use $v$ to denote the
generalized velocity vector of the plant. While a spatial velocity $^AV^B$
is six components, a translational or angular velocity, $^B\text{v}^C$ or
$^B\omega^C$, is three components, the generalized velocity vector is whatever
size it needs to be to encode the time derivatives of the configuration
variables, $q$. For the iiwa welded to the world frame, that means it has
seven components. I've tried to be careful to typeset each of these v's
differently throughout the notes. Almost always the distinction is also
clear from the context.</p>
<example><h1>Don't assume $\dot{q} \equiv v$</h1>
<p>The unit quaternion representation is four components, but these must
form a "unit vector" of length 1. Rotation matrices are 9 components, but
they must form an orthonormal matrix with $\det(R)=1$. It's pretty great
that for <i>changes</i> in rotation, we can use an <i>unconstrained</i>
three component vector, what we've called the angular velocity vector,
$\omega$. And you really should use it; getting rid of that constraint
makes both the math and the numerics better.</p>
<p>But there is one minor nuisance that this causes. We tend to want to
think of the generalized velocity as the time derivative of the
generalized positions. This works when we have only our iiwa in the
model, and it is welded to the world frame. But we cannot assume this in
general; not when floating-base rotations are concerned. As evidence,
here is a simple example that loads exactly one rigid body into the
<code>MultibodyPlant</code>, and then prints its
<code>Context</code>.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="qdot_vs_v"))</script>
<p>The output looks like this:
<code><pre>Context
--------
Time: 0
States:
13 continuous states
1 0 0 0 0 0 0 0 0 0 0 0 0
plant.num_positions() = 7
plant.num_velocities() = 6</pre></code></p>
<p>You can see that this system has 13 total state variables. 7 of them
are positions, $q$; we use unit quaternions in the position vector. But
we have just 6 velocities, $v$; we use angular velocities in the
velocity vector. Clearly, if the length of the vectors don't even match,
we do <i>not</i> have $\dot{q} = v$.</p>
<p>It's not really any harder; Drake provides the
<code>MultibodyPlant</code> methods <code>MapQDotToVelocity</code> and
<code>MapVelocityToQDot</code> to go back and forth between them. But you
have to remember to use them!</p>
</example>
<p>Due to the multiple possible representations of 3D rotation, and the
potential difference between $\dot{q}$ and $v$, there are actually
<b>many</b> different kinematic Jacobians possible. You may hear the terms
"analytic Jacobian", which refers to the explicit partial derivative of the
forward kinematics (as written in \eqref{eq:jacobian}), and "geometric
Jacobian" which replaces 3D rotations on the left-hand side with spatial
velocities. In Drake's <code>MultibodyPlant</code>, we currently offer the geometric Jacobian versions via
<ul>
<li> <code>CalcJacobianAngularVelocity</code>,</li>
<li><code>CalcJacobianTranslationalVelocity</code>, and</li>
<li><code>CalcJacobianSpatialVelocity</code>,</li>
</ul> with each taking an argument to specify whether you'd like the
Jacobian with respect to $\dot{q}$ or $v$. If you really like the
analytical Jacobian, you could get it (much less efficiently) using our
support for automatic differentiation.</p>
<example><h1>Kinematic Jacobians for pick and place</h1>
<p>Let's repeat the setup from above, but we'll print out the Jacobian of the gripper frame, measured from the world frame, expressed in the world frame.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="jacobian"))</script>
</example>
</section>
<section><h1>Differential inverse kinematics</h1>
<p>There is important structure in Eq \eqref{eq:jacobian}. Make sure you
didn't miss it. The relationship between joint velocities and end-effector
velocities is (configuration-dependent) linear: \begin{equation}V^G =
J^G(q)v.\end{equation} Maybe this gives us what we need to produce changes
in gripper frame $G$? If I have a desired gripper frame velocity $V^{G_d}$,
then how about commanding a joint velocity $v = \left[J^G(q)\right]^{-1}
V^{G_d}$?</p>
<subsection><h1>The Jacobian pseudo-inverse</h1>
<p>Any time you write a matrix inverse, it's important to check that the
matrix is actually invertible. As a first sanity check: what are the
dimensions of $J^G(q)$? We know the spatial velocity has six components.
Our gripper frame is welded directly on the last link of the iiwa, and
the iiwa has seven positions, so we have $J^G(q_{iiwa}) \in \Re^{6 \times
7}.$ The matrix is not square, so does not have an inverse. But having
<i>more</i> degrees of freedom than the desired spatial velocity requires
(more columns than rows) is actually the good case, in the sense that we
might have many solutions for $v$ that can achieve a desired spatial
velocity. To choose one of them (the minimum-norm solution), we can
consider using the <a
href="https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_inverse">Moore-Penrose
pseudo-inverse</a>, $J^+$, instead: \begin{equation}v =
[J^G(q)]^+V^{G_d}.\end{equation}</p>
<p>The pseudo-inverse is a beautiful mathematical concept. When the $J$
is square and full-rank, the pseudo-inverse returns the true inverse of
the system. When there are many solutions (here many joint velocities that
accomplish the same end-effector spatial velocity), then it returns the
minimum-norm solution (the joint velocities that produce the desired
spatial velocity which are closest to zero in the least-squares sense).
When there is no exact solution, it returns the joint velocities that
produce an spatial velocity which is as close to the desired end-effector
velocity as possible, again in the least-squares sense. So good!</p>
<example><h1>Our first end-effector "controller"</h1>
<p>Let's write a simple controller using the pseudo-inverse of the
Jacobian. First, we'll write a new <code>LeafSystem</code> that defines
one input port (for the iiwa measured position), and one output port
(for the iiwa joint velocity command). Inside that system, we'll ask
MultibodyPlant for the gripper Jacobian, and compute the joint
velocities that will implement a desired gripper spatial velocity.</p>
<div>
<script type="text/javascript">
var pinv = { "name": "PseudoInverseController",
"input_ports" : ["iiwa_position"],
"output_ports" : ["iiwa_velocity_command"]
};
document.write(system_html(pinv));
</script>
</div>
<p>To keep things simple for this first example, we'll just command a
constant gripper spatial velocity, and only run the simulation for a few
seconds.</p>
<p>Note that we do have to add one additional system into the diagram.
The output of our controller is a desired joint velocity, but the input
that the iiwa controller is expecting is a desired joint position. So
we will insert an integrator in between.</p>
<p>I don't expect you to understand every line in this example, but it's
worth finding the important lines and making sure you can change them
and see what happens!</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="pseudoinverse"))</script>
<p>Congratulations! Things are really moving now.</p>
</example>
</subsection>
<subsection><h1>Invertibility of the Jacobian</h1>
<p>There is a simple check to understand when the pseudo-inverse can give
an exact solution for any spatial velocity (achieving exactly the desired
spatial velocity): the Jacobian must be full <i>row</i>-rank. In this
case, we need $\rank(J) = 6$. But assigning an integer rank to a matrix
doesn't tell the entire story; for a real robot with (noisy)
floating-point joint positions, as the matrix gets <i>close</i> to losing
rank, the (pseudo-)inverse starts to "blow-up". A better metric, then,
is to watch the smallest <a
href="https://en.wikipedia.org/wiki/Singular_value_decomposition">singular
value</a>; as this approaches zero, the norm of the pseudo-inverse will
approach infinity.</p>
<example><h1>Invertibility of the gripper Jacobian</h1>
<p>You might have noticed that I printed out the smallest singular value
of $J^G$ in one of the previous examples. Take it for another spin.
See if you can find configurations where the smallest singular value
gets close to zero.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="jacobian"))</script>
<p>Here's a hint: try some configurations where the arm is very
straight, (e.g. driving joint 2 and 4 close to zero).</p>
<p>Another good way to find the singularities are to use your
pseudo-inverse controller to send gripper spatial velocity commands that
drive the gripper to the limits of the robot's workspace. Try it and
see! In fact, this is the common case, and one that we will work hard
to avoid.</p>
</example>
<p>Configurations $q$ for which $\rank(J(q_{iiwa})) < 6$ for a frame of
interest (like our gripper frame $G$) are called <i>kinematic
singularities</i>. Try to avoid going near them if you can! The iiwa has
many virtues, but admittedly its kinematic workspace is not one of them.
Trust me, if you try to get a big Kuka to reach into a little kitchen sink
all day, every day, then you will spend a non-trivial amount of time
thinking about avoiding singularities.</p>
<p>In practice, things can get a lot better if we stop bolting our robot
base to a fixed location in the world. Mobile bases add complexity, but
they are wonderful for improving the kinematic workspace of a robot.</p>
<example id="singularities"><h1>Are kinematic singularities real?</h1>
<p>A natural question when discussing singularities is whether they are
somehow real, or whether they are somehow an artifact of the analysis.
Perhaps it is useful to look at an extremely simple case.</p>
<p>Imagine a two-link arm, where each link has length one. Then the
kinematics reduce to $$p^G = \begin{bmatrix} c_0 + c_{0+1} \\ s_0 +
s_{0 + 1} \end{bmatrix},$$ where I've used the (very common) shorthard
$s_0$ for $\sin(q_0)$ and $s_{0+1}$ for $\sin(q_0+q_1)$, etc. The
<i>translational</i> Jacobian is $$J^G(q) = \begin{bmatrix} -s_0 -
s_{0+1} & -s_{0 + 1} \\ c_0 + c_{0 + 1} & c_{0 + 1} \end{bmatrix},$$
and as expected, it loses rank when the arm is at full extension (e.g.
when $q_0 = q_1 = 0$ which implies the first row is zero).</p>
<figure>
<iframe style="border:0;height:300px;width:400px;" src="data/two_link_singularities.html" pdf="no"></iframe>
<p pdf="only"><a href="data/two_link_singularities.html">Click here for the animation.</a></p>
</figure>
<p>Let's move the robot along the $x$-axis, by taking $q_0(t) = 1-t,$
and $q_1(t) = -2 + 2t$. This clearly visits the singularity $q_0 = q_1
= 0$ at time 1, and then leaves again without trouble. In fact, it does
all this with a <i>constant</i> joint velocity ($\dot{q}_0=-1,
\dot{q}_1=2$)! The resulting trajectory is $$p^G(t) = \begin{bmatrix}
2\cos(1-t) \\ 0 \end{bmatrix}.$$</p>
<p>There are a few things to understand. At the singularity, there is
nothing that the robot can do to move its gripper farther in positive
$x$ -- that singularity is real. But it is also true that there is no
way for the robot to move instantaneously back in the direction of
$-x.$ The Jacobian analysis is not an approximation, it is a perfect
description of the relationship between joint velocities and gripper
velocities. However, just because you cannot achieve an instantaneous
velocity in the backwards direction, it does not mean you cannot get
there! At $t=1$, even though the joint velocities are constant, and
the translational Jacobian is singular, the robot is
<i>accelerating</i> in $-x$, $\ddot{p}^G_{W_x}(t) = -2\cos(1-t).$</p>
<todo>Update this to use the two-link iiwa (like i've done in the
qp_diff_ik notebook). The link lengths aren't *quite* one to one, but
it's close. could have a point on the second link that is the right
distance away, or just add the ratio logic here.</todo>
</example>
<p>For the cases when the Jacobian does not have a unique inverse, there
is an interesting subtlety to be aware of regarding <i>integrability</i>.
Let's start our robot in at time zero in a joint configuration, $q(0)$
with a corresponding in an end-effector pose $X^G(0)$. Now let us execute
an end-effector trajectory using the pseudo-inverse controller that moves
along a closed-path in the task space, coming back to the original
end-effector pose at time one: $X^G(1) = X^G(0)$. Unfortunately, there is
no guarantee that $q(1) = q(0)$; this is not simply due to numerical
errors in our numerical implementation, it is well known that the
pseudo-inverse does not in general produce an integrable function.
<elib>Mussa-Ivaldi91</elib> discusses this thoroughly, and proposed a
weighted pseudo-inverse that can restore the integrability conditions by
means of a virtual "compliance function".</p>
</subsection>
</section>
<section><h1>Defining the grasp and pre-grasp poses</h1>
<p>I'm going to put my red foam brick on the table. Its geometry is <a
href="https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/models/061_foam_brick.sdf#L21">defined</a>
as a 7.5cm x 5cm x 5cm box. For reference, the distance between the fingers
on our gripper in the default "open" position is 10.7cm. The "palm" of the
gripper is 3.625cm from the body origin, and the fingers are 8.2cm long.</p>
<p>To make things easy to start, I'll promise to set the object down on the
table with the object frame's $z$-axis pointing up (aligned with the world
$z$ axis), and you can assume it is resting on the table safely within the
workspace of the robot. But I reserve the right to give the object
arbitrary initial yaw. Don't worry, you might have noticed that the
seventh joint of the iiwa will let you rotate your gripper around quite
nicely (well beyond what my human wrist can do).</p>
<p>Observe that visually the box has rotational symmetry -- I could always
rotate the box 90 degrees around its $x$-axis and you wouldn't be able to
tell the difference. We'll think about the consequences of that more in the
next chapter when we start using perception. But for now, we are ok using
the omniscient "cheat port" from the simulator which gives us the
unambiguous pose of the brick.</p>
<figure id="grasp_frames">
<img style="width:40%" src="data/grasp_frames.png"/>
<figcaption>The gripper frame and the object frame. For each frame, the
<span style="color:red">positive $x$ axis is in red</span>, the <span
style="color:green">positive $y$ axis is in green</span>, and the <span
style="color:blue">positive $z$ axis is in blue</span> (XYZ $\Leftrightarrow$
RGB).</figcaption>
</figure>
<p>Take a careful look at the gripper frame in the figure above, using the
colors to understand the axes. Here is my thinking: Given the size of the
hand and the object, I want the <i>desired</i> position (in meters) of the
object in the gripper frame, $${}^{G_{grasp}}p^O = \begin{bmatrix} 0 \\ 0.11
\\ 0 \end{bmatrix}, \qquad {}^{G_{pregrasp}}p^O = \begin{bmatrix} 0 \\ 0.2
\\ 0 \end{bmatrix}.$$ Recall that the logic behind a <i>pregrasp</i> pose
is to first move to safely above the object, if our only gripper motion that
is very close to the object is a straight translation from the pregrasp pose
to the grasp pose and back, then it allows us to mostly avoid having to
think about collisions (for now). I want the orientation of my gripper to
be set so that the positive $z$ axis of the object aligns with the negative
$y$ axis of the gripper frame, and the positive $x$ axis of the object
aligns with the positive $z$ axis of the gripper. We can accomplish that
with $${}^{G_{grasp}}R^O = \text{MakeXRotation}\left(\frac{\pi}{2}\right)
\text{MakeZRotation} \left(\frac{\pi}{2}\right).$$ I admit I had my right
hand in the air for that one! Our pregrasp pose will have the same
orientation as our grasp pose.</p>
<example><h1>Computing grasp and pregrasp poses</h1>
<figure>
<img style="width:70%" src="figures/grasp_pose.png"/>
</figure>
<p>Here is a simple example of loading a floating Schunk gripper and a
brick, computing the grasp / pregrasp pose (drawing out each
transformation clearly), and rendering the hand relative to the
object.</p>
<script>document.write(notebook_link('pick', d=deepnote, link_text="", notebook="grasp"))</script>
<p>I hope you can see the value of having good notation at work here. My
right hand was in the air when I was deciding what a suitable relative
pose for the object in the hand should be (when writing the notes). But
once that was decided, I went to type it in and everything just
worked.</p>
</example>
</section>
<section><h1>A pick and place trajectory</h1>
<p>We're getting close. We know how to produce desired gripper poses, and
we know how to change the gripper pose instantaneously using spatial
velocity commands. Now we need to specify how we want the gripper poses to
change over time, so that we can convert our gripper poses into spatial
velocity commands.</p>
<p>Let's define all of the "keyframe" poses that we'd like the gripper to
travel through, and time that it should visit each one. The following
example does precisely that.</p>
<example><h1>A plan "sketch"</h1>
<script>document.write(notebook_link('pick'))</script>
<figure>
<img style="width:70%" src="figures/pick_sketch.png"/>
<figcaption>Keyframes of the gripper. The robot's base will be at the
origin, so we're looking over the (invisible) robot's shoulder here. The
hand starts in the "initial" pose near the center, moves to the
"prepick" to "pick" to "prepick" to "clearance" to "preplace" to "place"
and finally back to "preplace".</figcaption>
</figure>
<todo>timeline graphic here, from time zero, to pre-grasp, to grasp, to
</todo>
<p>How did I choose the times? I started everything at time, $t=0$, and
listed the rest of our times as absolute (time from zero). That's when the
robot wakes up and sees the brick. How long should we take to transition
from the starting pose to the pregrasp pose? A really good answer might
depend on the exact joint speed limits of the robot, but we're not trying
to move fast yet. Instead I chose a conservative time that is
proportional to the total Euclidean distance that the hand will travel,
say $k=10~s/m$ (aka $10~cm/s$): $$t_{pregrasp} = k
\left\|{}^{G_0}p^{G_{pregrasp}}\right\|_2.$$ I just chose a fixed
duration of 2 seconds for the transitions from pregrasp to grasp and back,
and also left 2 seconds with the gripper stationary for the segments where
the fingers needs to open/close.</p>
</example>
<p>There are a number of ways one might represent a trajectory
computationally. We have a pretty good collection of
<a
href="https://drake.mit.edu/doxygen_cxx/namespacedrake_1_1trajectories.html">trajectory</a>
classes available in Drake. Many of them are implemented as <a
href="https://en.wikipedia.org/wiki/Spline_(mathematics)">splines</a> --
piecewise polynomial functions of time. Interpolating between orientations
requires some care, but for positions we can do a simple linear
interpolation between each of our keyframes. That would be called a "<a
href="https://en.wikipedia.org/wiki/First-order_hold">first-order hold</a>",
and it's implemented in Drake's <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_polynomial.html">PiecewisePolynomial</a>
class. For rotations, we'll use something called "spherical linear
interpolation" or <a href="https://en.wikipedia.org/wiki/Slerp">slerp</a>,
which is implemented in Drake's <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_quaternion_slerp.html">PiecewiseQuaternionSlerp</a>, and which you can explore in <a href="#slerp">this exercise</a>. The <a href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_pose.html">PiecewisePose</a> class makes it convenient to construct and work with the position and orientation trajectories together.</p>
<example><h1>Grasping with trajectories</h1>
<p>There are a number of ways to visualize the trajectory when it's
connected to 3D. I've plotted the <i>position</i> trajectory as a
function of time below.<p>
<script>document.write(notebook_link('pick'))</script>
<figure>