Repository navigation
Expand file tree
/
Copy pathforce.html
More file actions
1176 lines (989 loc) · 67.6 KB
/
Copy pathforce.html
File metadata and controls
1176 lines (989 loc) · 67.6 KB
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. 8 - Manipulator Control</title>
<meta name="Ch. 8 - Manipulator Control" content="text/html; charset=utf-8;" />
<link rel="canonical" href="http://manipulation.csail.mit.edu/force.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-2023<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/Fall2023/">a course being taught
at MIT</a>. They will be updated throughout the Fall 2023 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=trajectories.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=segmentation.html>Next Chapter</a></td>
</tr></table>
<script type="text/javascript">document.write(notebook_header('force'))
</script>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 7">
<h1>Manipulator Control</h1>
<p>Over the last few chapters, we've developed a great initial toolkit for
perceiving objects (or piles of objects) in a scene, planning grasps, and
moving the arm to grasp. In the last chapter, we started designing motion
planning strategies that could produce beautiful smooth joint trajectories
for the robot that could move it quickly from one grasp to the next. As our
desired trajectories get closer to the dynamic limits of the robot, then the
control strategies that we use to execute them on the robot arm also need to
get a bit more sophisticated.</p>
<p>Remember, too, that there is a lot more to manipulation than grasping!
Even in the bin picking application, we already have some examples where our
grasping-only strategy can be insufficient. Imagine that you look through the
robot cameras into the box and see the following scene:</p>
<figure><img style="width:60%" src="data/09954.png"/></figure>
<p>How in the world are we going to pick up that "Cheez-it" cracker box with
our two-fingered gripper? (I know there are a few of you out there thinking
about how easy that scene is for your suction gripper, but suction alone
isn't going to solve all of our problems either.)</p>
<figure>
<iframe width="560" height="315"
src="https://www.youtube.com/embed/XqRczI4Fepk?mute=1&rel=0" frameborder="0"
allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope;
picture-in-picture" allowfullscreen pdf="no"></iframe>
<p pdf="only"><a href="https://www.youtube.com/embed/XqRczI4Fepk">Click here to watch the video.</a></p>
<figcaption>Just for fun, I asked my daughter to try a similar task with the
"two-fingered gripper" I scrounged from my kitchen. How are we to program
something like that!</figcaption></figure>
<p>The term <i>nonprehensile manipulation</i> means "manipulation without
grasping", and humans do it often. It is easy to appreciate this fact when we
think about pushing an object that is too big to grasp (e.g. an office chair).
But if you pay close attention, you will see that humans make use of
strategies like sliding and environmental contact often, even when they are
grasping. These strategies just come to the forefront in non-grasping
manipulation.</p>
<p>As we build up our toolkit for prehensile (grasping) and nonprehensile
manipulation, one of the things that is missing from our discussion so far,
which has been predominantly kinematic, has been thinking about forces. In
addition to tracking motion planning trajectories, this chapter will also
introduce control techniques that reason about contact forces. I hope that by
the end you'll agree that we have pretty satisfying approaches to flipping up
that box!</p>
<section id="toolbox"><h1>The Manipulator-Control Toolbox</h1>
<p>When I talk about "manipulator control", I am discussing robot
controllers that take (slightly) higher-level commands, like desired joint
positions and velocities or spatial forces, and convert them into motor
commands. Throughout this chapter we will assume that we can command
<i>generalized forces</i>, which for revolute-joint robots are just joint
torques. These controllers, by themselves, are not enough to complete any
meaningful task -- they are reasoning about the robot itself but not about
any of the objects in the environment. But they facilitate writing the rest
of the control systems by providing the higher-level abstraction.</p>
<p>Typically the low-level controllers we will be discussing here are
implemented in the firmware that runs directly on the robot arm (or its
control cabinet). For the purposes of using that hardware, it's important
to understand how they work and how you should set their parameters. In
simulation we need to implement these controllers ourselves in order to
model the robot.</p>
<p>Drake offers a number of <a
href="https://drake.mit.edu/doxygen_cxx/group__control__systems.html">
manipulator control implementations</a>. In this chapter, we will work
through the most relevant ones for controlling a robotic arm:</p>
<figure>
<div style="padding: 10px;">
<script type="text/javascript">
var system = { "name": "PidController",
"input_ports" : ["estimated_state", "desired_state"],
"output_ports" : ["generalized_force"]
};
document.write(system_html(system, "https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_pid_controller.html"));
</script>
</div><div style="padding: 10px;">
<script type="text/javascript">
var system = { "name": "InverseDynamicsController",
"input_ports" : ["estimated_state", "desired_state", '<span style="color:gray">desired_acceleration</span>'],
"output_ports" : ["generalized_force"]
};
document.write(system_html(system, "https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_inverse_dynamics_controller.html"));
</script>
</div><div style="padding: 10px;">
<script type="text/javascript">
var system = { "name": "SpatialForceController",
"input_ports" : ["estimated_state", "desired_spatial_force"],
"output_ports" : ["generalized_force"]
};
document.write(system_html(system));
</script>
</div><div style="padding: 10px;">
<script type="text/javascript">
var system = { "name": "JointStiffnessController",
"input_ports" : ["estimated_state", "desired_state"],
"output_ports" : ["generalized_force"]
};
document.write(system_html(system, "https://github.com/RobotLocomotion/drake/pull/18272"));
</script>
</div><div style="padding: 10px;">
<script type="text/javascript">
var system = { "name": "SpatialStiffnessController",
"input_ports" : ["estimated_state", "desired_pose", "desired_spatial_velocity", "secondary_task"],
"output_ports" : ["generalized_force"]
};
document.write(system_html(system));
</script>
</div>
</figure>
<p>By the end of the chapter, I hope you'll understand the differences and
the essentials of how they all work.</p>
</section>
<section><h1>Assume your robot is a point mass</h1>
<p>As always in our battle against complexity, I want to find a setup that
is as simple as possible (but no simpler)! Here is my proposal for the
box-flipping example. First, I will restrict all motion to a 2D plane; this
allows me to chop off two sides of the bin (for you to easily see inside),
but also drops the number of degrees of freedom we have to consider. In
particular, we can avoid the quaternion floating base coordinates, by adding
a <code>PlanarJoint</code> to the box. Instead of using a complete gripper,
let's start even simpler and just use a "point finger". I've visualized it
as a small sphere, and modeled two control inputs as providing force
directly on the $x$ and $z$ coordinates of the point mass.</p>
<figure><img style="width:60%"
src="data/point_finger_cracker_box.png"/><figcaption>The simple model: a
point finger, a cracker box, and the bin all in 2D. The green arrows are
the contact forces.</figcaption></figure>
<p>Even in this simple model, and throughout the discussions in this
chapter, we will have two dynamic models that we care about. The first is
the full dynamic model used for simulation: it contains the finger, the
box, and the bin, and has a total of 5 degrees of freedom ($x, z, \theta$
for the box and $x, z$ for the finger). The second is the robot model used
for designing the controllers: this model has just the two degrees of
freedom of the finger, and experiences unmodelled contact forces. By
design, the equations of this second model are particularly simple:
$$\begin{bmatrix}m & 0 \\ 0 & m \end{bmatrix} \dot{v} = m
\begin{bmatrix}\ddot{x} \\ \ddot{z} \end{bmatrix} = \tau_g +
\begin{bmatrix} u_x \\ u_z \end{bmatrix} + f^{F_c},$$ where $m$ is the
mass, $\tau_g$ is the gravitational "torque" which here is just $\tau_g =
[0, -mg]^T$, $u$ is the control input vector, and $f^{F_c}$ is the
Cartesian contact force applied to the finger, $F$, at $c$.</p>
<p>Just to make the notation clear, I've used $f^{F_c}$ above to denote
force <i>applied to the finger</i>, $F$, at point $c$. If we want to denote
the same force <i>applied to the box</i>, $B$, at point $c$, then we'll
denote it as $f^{B_c}$. Naturally, we have $f^{F_c} = - f^{B_c}$, because
the forces must be equal and opposite. If we're not careful about this
notation, the signs can get confusing below.</p>
<subsection><h1>Trajectory tracking</h1>
<p>Before we even make contact with the box, let's make sure we know how
to move our robot finger around through the air. Assume that you've done
some motion planning, perhaps using optimization, and have developed a
beautiful desired trajectory, $q_d(t)$, that you want your finger to
follow. Let's assume that $q_d(t)$ is twice differentiable. What
generalized-force commands should you send to the robot to execute that
trajectory?</p>
<p>One of the first and most common ways to execute the trajectory is
with proportional-integral-derivative (PID) control, which we've
discussed when we talked about <a
href="robot.html#position-controlled">position-controlled robots</a>. The
<a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_pid_controller.html">PidController</a>
uses the command $$u(t) = K_p (q_d(t) - q(t)) + K_d (\dot{q}_d(t) -
\dot{q}(t)) + K_i \int_0^t (q_d(t) - q(t)) dt,$$ with $K_p$, $K_d$, and
$K_i$ being the position, velocity, and integral gains. Often these gains
are diagonal matrices, making the command for each joint independent of
the rest. Note that in manipulation, we tend to avoid the integral term,
setting $K_i = 0$. You can imagine that if the robot is pushing up
against the wall and is unable to achieve $q(t) == q_d(t)$, then the
integral term will "wind-up", sending larger and larger commands to the
robot until some fault is reached.</p>
<p>PD/PID control can be incredibly effective; it assumes almost nothing
about the dynamics of the robot. But this assumption is also a
limitation; if we do know something about the dynamics of the robot than
we can achieve better tracking performance. To make that clear, let's
write the closed-loop dynamics under PD control...</p>
<p>Gravity compensation... For reasons that will become clear soon, this
controller is available in Drake as the <a
href="https://github.com/RobotLocomotion/drake/pull/18272">JointStiffnessController</a>.</p>
<p>Inverse dynamics control...</p>
<example><h1>Trajectory tracking for the point mass</h1>
<p>I've put together an extremely simple example to show the differences in tracking performance for the point mass using each of these control strategies.</p>
<script>document.write(notebook_link('force', d = deepnote, link_text = "", notebook = "tracking"))</script>
<todo>insert the figure here</todo>
</example>
</subsection>
<subsection><h1>(Direct) force control</h1>
<p>In the case of direct force control, we implement a low-level
controller that accepts the desired contact forces and produces
generalized force inputs to the robot that try to make the actual contact
forces match the desired.</p>
<div>
<script type="text/javascript">
var force_control = { "name": "ForceControl",
"input_ports" : ["estimated_state", "desired_contact_force", '<span style="color:gray">measured_accelerations</span>', '<span style="color:gray">measured_contact_force</span>'],
"output_ports" : ["generalized_force"]
};
document.write(system_html(force_control));
</script>
</div>
<p>What information do we need to regulate the contact forces? Certainly
we need the desired contact force, $f^{F_c}_{desired}$. In general, we
will need the robot's state (though in the immediate example, the
dynamics of our point mass are not state dependent). But we also need to
either (1) measure the robot accelerations (which we've repeatedly tried
to avoid), (2) assume the robot accelerations are zero, or (3) provide a
measurement of the contact force so that we can regulate it with
feedback.</p>
<p>Let's consider the case where the robot is already in contact with the
box. Let's also assume for a moment that the accelerations are (nearly)
zero. This is actually not a horrible assumption for most manipulation
tasks, where the movements are relatively slow. In this case, our
equations of motion reduce to $$f^{F_c} = - mg - u.$$ Our force
controller implementation can be as simple as $u = -mg -
f^{F_c}_{desired}.$ Note that we only need to know the mass <i>of the
robot</i> (not the box) to implement this controller.</p>
<p>What happens when the robot is not in contact? In this case, we
cannot reasonably ignore the accelerations, and applying the same control
results in $m\dot{v} = - f^{F_c}_{desired}.$ That's not all bad. In
fact, it's one of the defining features of force control that makes it
very appealing. When you specify a desired force and don't get it, the
result is accelerating the contact point in the (opposite) direction of
the desired force. In practice, this (locally) tends to bring you into
contact when you are not in contact.</p>
<example><h1>Commanding a constant force</h1>
<p>Let's see what happens when we run a full simulation which includes
not only the non-contact case and the contact case, but also the
transition between the two (which involves collision dynamics). I'll
start the point finger next to the box, and apply a constant force
command requesting to get a horizontal contact force from the box. I've
drawn the $x$ trajectory of the finger for different (constant) contact
force commands.</p>
<figure><img style="margin-top:-20px; width:70%"
src="data/constant_force_box.svg"><figcaption>This is a plot of the
horizontal position of the finger as a function of time, given different
constant desired force commands.</figcaption></figure>
<p>For all strictly positive desired force commands, the finger will
accelerate at a constant rate until it collides with the box (at
$x=0.089$). For small $f^{F_c}_{desired}$, the box barely moves. For
an intermediate range of $f^{F_c}_{desired}$, the collision with the
box is enough to start it moving, but friction eventually brings the
box and therefore also the finger to rest. For large values, the
finger will keep pushing the box until it runs into the far wall.</p>
<script>document.write(notebook_link('force', d = deepnote, link_text = "", notebook = "point_finger"))</script>
</example>
<p>Consider the consequences of this behavior. By commanding force, we
can write a controller that will come into a nice contact with the box
with essentially no information about the geometry of the box (we just
need enough perception to start our finger in a location for which a
straight-line approach will reach the box).</p>
<p>This is one of the reasons that researchers working on legged robots
also like force control. On a force-capable walking robot, we might mimic
position control during the "swing phase", to get our foot approximately
where we are hoping to step. But then we switch to a force control mode
to actually set the foot down on the ground. This can significantly
reduce the requirements for accurate sensing of the terrain.</p>
<example id="force_flip_up"><h1>A force-based flip-up strategy</h1>
<todo>Update the code to match this updated derivation. In particular,
\theta_d(t) should get more negative.</todo>
<p>Here is my proposal for a simple strategy for flipping up the box.
Once in contact, we will use the contact force from the finger to apply
a moment around the bottom left corner of the box to generate the
rotation. But we'll add constraints to the forces that we apply so that
the bottom corner does not slip. </p>
<figure><img style="margin-top:-20px; width:60%"
src="data/force_flip_up.png"><figcaption>Flipping the box. <a
href="data/force_flip_up.html">Click here</a> for an animation of the
controller we'll write in the example below. But it's worth running
the code, where you can see the contact force visualization,
too!</figcaption></figure>
<p>These conditions are very natural to express in terms of forces.
And once again, we can implement this strategy with very little
information about the box. The position of the finger will evolve
naturally as we apply the contact forces. It's harder for me to
imagine how to write an equally robust strategy using a (high-gain)
position-controller finger; it would likely require many more
assumptions about the geometry (and possibly the mass) of the box.</p>
<p>Let's encode the textual description above, describing the forces
that are applied to the box. I'll use $C$ for the contact frame
between the finger and the box, with its normal pointing <i>into</i>
the box normal to the surface, and $A$ for the contact frame for the
lower left corner of the box contacting the bin, with the normal
pointing straight up in positive world $z$. $${}^BR^C =
R_y(-\frac{\pi}{2}), \qquad {}^WR^A = I,$$ where $R_y(\theta)$ is a
rotation by $\theta$ around the y axis. Of course we also have the
force of gravity, which is applied at the body center of mass (com):
$$f^{B_{com}}_{gravity,W} = mg.$$ As you can see, we'll make heavy use
of the spatial force notation / spatial algebra <a
href="clutter.html#spatial_force">described here</a>.</p>
<p>The friction cone provides (linear inequality) constraints on the
forces we want to apply. \begin{gather*} f^{B_C}_{\text{finger}, C_z}
\ge 0, \qquad |f^{B_C}_{\text{finger}, C_x}| \le \hat\mu_C
f^{B_C}_{\text{finger}, C_z}, \\ f^{B_A}_{\text{ground}, A_z} \ge 0,
\qquad |f^{B_A}_{\text{ground}, A_x}| \le \hat\mu_A
f^{B_A}_{\text{ground}, A_z}.\end{gather*} Please make sure you
understand the notation. Within those constraints, we would like to
rotate up the box.</p>
<p>To rotate the box about $A$, let's reason about the total torque
being applied about $A$: $$\tau^{B_A}_{total,W} = \tau^{B_A}_{gravity,
W} + \tau^{B_A}_{ground, W} + \tau^{B_A}_{finger, W},$$ but we know
that $\tau^{B_A}_{ground, W} = 0$ since the moment arm is zero. I have
a goal here of not making too many assumptions about the mass and
geometry of the box in our controller, so rather than try to regulate
this torque perfectly, let's write $$\tau^{B_A}_{total,W_y} =
\tau^{B_A}_{finger,W_y} + \text{unknown terms}.$$ <!--
and if we choose to
assume that the box is longer than it is tall (so ${}^Ap^C_{B_x} \gg
{}^Ap^C_{B_z}$), we can have approximately $$\tau^{B_A}_{finger,W_y}
\approx {}^A\hat{p}^C_{B_x} \, f^{B_C}_{finger, C_x}.$$ -->
A reasonable control strategy in the face of these unmodeled terms is
to use feedback control on the angle of the box (call it $\theta$)
which is the $y$ component of ${}^WR^B$: $$ \tau^{B_A}_{finger_d,W_y} =
\text{PID}(\theta_d, \theta), \qquad {}^WR^B(\theta) = R_y(\theta),$$
where I've used $\theta_{d}$ as shorthand for the desired box angle and
$\text{PID}$ as shorthand for a simple proportional-integral-derivative
term. Note that $\tau^{B_A}_{finger,W_y} \propto f^{B_C}_{finger,
C_x},$ where the (constant) coefficient only depends on
${}^B\hat{p}^C;$ it does not actually depend on $\hat\theta.$</p>
<p>To execute the desired PID control subject to the friction-cone
constraints, we can use constrained least-squares:
\begin{align*}\min_{f^{B_C}_{finger,C}, \, f^{B_A}_{ground,A}} \quad&
\left| \tau^{B_A}_{finger,W_y} - \text{PID}(\theta_d, \hat\theta)
\right|^2,\\ \subjto \quad & f^{B_C}_{\text{finger}, C_z} \ge 0, \qquad
|f^{B_C}_{\text{finger}, C_x}| \le \hat\mu_C f^{B_C}_{\text{finger},
C_z}, \\ & f^{B_A}_{\text{ground}, A_z} \ge 0, \qquad
|f^{B_A}_{\text{ground}, A_x}| \le \hat\mu_A f^{B_A}_{\text{ground},
A_z}, \\ & f^{B_A}_{\text{ground}, A} + \hat{f}^{B_A}_{gravity, A} +
f^{B_A}_{finger, A} = 0.\end{align*} Note that the last line is still a
linear constraint once $\hat\theta$ is given, despite requiring some
spatial algebra operations. Implementing this strategy assumes:
<ul style="margin-top:-10px; margin-bottom:5px"><li>We have some
approximation, $\hat\theta$, for the orientation of the box. We could
obtain this from a point cloud normal estimation, or even from tracking
the path of the fingers.</li>
<li>We have conservative estimates of the coefficients of static
friction between the wall and the box, $\hat\mu_A$, and between the
finger and the box, $\hat\mu_C$, as well as the approximate location of
the finger relative to the box corner, and the box mass,
$\hat{m}.$</li>
</ul> You should experiment and decide for yourself whether these
estimates can be loose. I think that you'll find it's quite robust to
knowing the mass and geometry of the box.</p>
<script>document.write(notebook_link('force', d = deepnote, link_text = "", notebook = "point_finger"))</script>
<todo>Generate random boxes.</todo>
<p>We have multiple controllers in this example. The first is the
low-level force controller that takes a desired contact force and sends
commands to the robot to attempt to regulate this force. The second is
the higher-level controller that is looking at the orientation of the
box and deciding which forces to request from the low-level
controller.</p>
<p>Please also understand that this is not some unique or optimal strategy
for box flipping. I'm simply trying to demonstrate that sometimes
controllers which might be difficult to express otherwise can easily be
expressed in terms of forces!</p>
</example>
</subsection>
<subsection><h1>Indirect force control</h1>
<p>There is a nice philosophical alternative to controlling the contact
interactions by specifying the forces directly. Instead, we can program
our robot to act like a (simple) mechanical system that reacts to contact
forces in a desired way. This philosophy was described nicely in an
important series of papers by Ken Salisbury introducing <i>stiffness
control</i> <elib>Salisbury80</elib> and then Neville Hogan introducing
<i>impedance control</i> <elib>Hogan85a+Hogan85b+Hogan85c</elib>.</p>
<p>This approach is conceptually very nice. Imagine we were to walk up
and push on the end-effector of the iiwa. With only knowledge of the
parameters of the robot itself (not the environment), we can write a
controller so that when we push on the end-effector, the end-effector
pushes back (using the entire robot) as if you were pushing on, for
instance, a simple spring-mass-damper system. Rather than attempting to
achieve manipulation by moving the end-effector rigidly through a series
of position commands, we can move the set points (and perhaps stiffness)
of a soft virtual spring, and allow this virtual spring to generate our
desired contact forces.</p>
<p>This approach can also have nice advantages for guaranteeing that your
robot won't go unstable even in the face of unmodeled contact
interactions. If the robot acts like a dissipative system and the
environment is a dissipative system, then the entire system will be
stable. Arguments of this form can ensure stability for even very complex
system, building on the rich literature on <i>passivity theory</i> or more
generally <i>Port-Hamiltonian</i>
systems<elib>Duindam09</elib>.</p>
<p>Our simple model with a point finger is ideal for understanding the
essence of indirect force control. The original equations of motion of
our system are $$m\begin{bmatrix}\ddot{x} \\ \ddot{z} \end{bmatrix} = mg
+ u + f^{F_c}.$$ We can write a simple controller to make this system
act, instead, like (passive) mass-spring-damper system: $$m
\begin{bmatrix}\ddot{x} \\ \ddot{z} \end{bmatrix} + b \begin{bmatrix}
\dot{x} \\ \dot{z} \end{bmatrix} + k \begin{bmatrix} x - x_d \\ z - z_d
\end{bmatrix} = f^{F_c},$$ with the rest position of the spring at $(x_d,
z_d).$ The controller that implements this follows easily; in the point
finger case this has the familiar form of a proportional-derivative (PD)
controller, but with an additional "feed-forward" term to cancel out
gravity.</p>
<p>Technically, if we are just programming the stiffness and damping, as
I've written here, then a controller of this form would commonly be
referred to as "stiffness control", which is a subset of impedance
control. We could also change the effective mass of the system; this
would be impedance control in its full glory. <!--Importantly, changing
the effective mass can be done without estimating the accelerations, but
it does require measuring the contact force.--> My impression, though,
is that the consensus in robot control experts is that changing the
effective mass is most often considered not worth the complexity that
comes from the extra sensing and bandwidth requirements.</p>
<p>The literature on indirect force control has a lot of terminology and
implementation details that are important to get right in practice. Your
exact implementation will depend on, for instance, whether you have access
to a force sensor and whether you can command forces/torque directly. The
performance can vary significantly based on the bandwidth of your
controller and the quality of your sensors. See e.g.
<elib>Villani08</elib> for a more thorough survey (and also some fun older
videos), or <elib>Whitney87</elib> for a nice earlier perspective.</p>
<!--
<p>We can also program the response if we have a position-controlled robot with a force-torque sensor. This is called <i>admittance control</i>. In its simplest form, we can say that the original equations of motion already contain a PD controller, and our only command input is to modulate the setpoints of that controller. For a single degree of freedom, we have $$m\ddot{q} + \text{unmodelled terms} = k_p(q_d - q) + k_d(\dot{q}_d - \dot{q}) + f^c.$$ </p>
-->
<example><h1>Teleop with stiffness control</h1>
<p>I didn't give you a teleop interface with direct force control; it
would have been very difficult to use! Moving the robot by positioning
the set points on virtual springs, however, is quite natural. Take a
minute to try moving the box around, or even flipping it up.</p>
<script>document.write(notebook_link('force', d = deepnote, link_text = "", notebook = "point_finger"))</script>
<p>To help your intuition, I've made the bin and the box slightly
transparent, and added a visualization (in orange) of the virtual finger
or gripper that you are moving with the sliders.</p>
</example>
<p>Let's embrace indirect force control to come up with another approach
to flipping up the box. Flipping the box up in the middle of the bin
required very explicit reasoning about forces in order to stay inside the
friction cone in the bottom left corner of the box. But there is another
strategy that doesn't require as precise control of the forces. Let's
push the box into the corner, and <i>then</i> flip it up.</p>
<figure><img style="width:70%" src="data/impedance-based-flip-up.jpeg"></figure>
<p>To make this one happen, I'd like to imagine creating a virtual spring
-- you can think of it like a taut rubber band -- that we attach from
the finger to a point near the wall just a little above the top left
corner of the box. The box will act like a pendulum, rotating around the
top corner, with the rubber band creating the moment. At some point the
top corner will slip, but the very same rubber band will have the finger
pushing the box down from the top corner to complete the maneuver.</p>
<p>Consider the alternative of writing an estimator and controller that
needed to detect the moment of slip and make a new plan. That is not a
road to happiness. By only using our model of the robot to make the robot
act like a different dynamical system at the point we can accomplish all
of that!</p>
<example><h1>A stiffness-control-based flip-up strategy</h1>
<p>This controller almost couldn't be simpler. I will just command a
trajectory the move the virtual finger to just in front of the wall.
This will push the box into contact and establish our bracing contact
force. Then I'll move the virtual finger (the other end of our rubber
band) up the wall a bit, and we can let mechanics take care of the
rest!</p>
<script>document.write(notebook_link('force', d = deepnote, link_text = "", notebook = "point_finger"))</script>
</example>
<p>So far we've made our finger act like two independent
mass-spring-damper systems, one in $x$ and the other in $z$. We even used
the same stiffness, $k$, and damping, $b$, parameters for each. More
generally, we can write stiffness and damping matrices, which we
typically would call $K_p$ and $K_d$, respectively: $$m
\begin{bmatrix}\ddot{x} \\ \ddot{z} \end{bmatrix} + K_d \begin{bmatrix}
\dot{x} \\ \dot{z} \end{bmatrix} + K_p \begin{bmatrix} x - x_d \\ z - z_d
\end{bmatrix} = f^{F_c}.$$ In order to emphasize the coordinate frame of
these stiffness matrices, let's write exactly the same equation,
realizing that with our point finger we have ${}^Wp^F = \begin{bmatrix}
x, z\end{bmatrix}^T,$ so: $$m\,{}^W\dot{v}^F + K_d \, {}^Wv^F +
K_p\left({}^Wp^F - {}^Wp^{F_d}\right) = f^F.$$ Sometimes it can be
convenient to express the stiffness (and/or damping) matrices in a
different frame, $A$: \begin{align*} m\,{}^W\dot{v}^F + {}^W R^A K_d \,
{}^Wv^F_A + {}^WR^W K_p\left({}^Wp^F_A - {}^Wp^{F_d}_A\right) =& \\
m\,{}^W\dot{v}^F + {}^W R^A K_d {}^A R^W\, {}^Wv^F + {}^WR^A K_p {}^A
R^W\left(\,{}^Wp^F - {}^Wp^{F_d}\right) =& \, f^F.\end{align*}</p>
</subsection>
<subsection><h1>Hybrid position/force control</h1>
<p>There are a number of applications where we would like to explicitly
command force in one direction, but command positions in another. One
classic examples if you are trying to wipe or polish a surface -- you
might care about the amount of force you are applying normal to the
surface, but use position feedback to follow a trajectory in the
directions tangent to the surface. In the simplest case, imagine
controlling force in $z$ and position in $x$ for our point finger: $$u =
-mg + \begin{bmatrix} k_p (x_d - x) + k_d (\dot{x}_d - \dot{x}) \\
-f^F_{desired, W_z} \end{bmatrix}.$$ If want the forces/positions in a
different frame, for instance the contact frame, $C$, then we can use
\begin{equation} u = -mg + {}^WR^C \begin{bmatrix} k_p (p^{F_d}_{C_x} -
p^{F}_{C_x}) + k_d (v^{F_d}_{C_x} - v^F_{C_x}) \\ -f^{F}_{desired,
C_z}\end{bmatrix}. \label{eq:position-or-force}\end{equation} By commanding the normal force, you not
only have the benefit of controlling how hard the robot is pushing on the
surface, but also gain some robustness to errors in estimating the
surface normal. If a position-controlled robot estimated the normal of a
wall badly, then it might leave the wall entirely in one direction, and
push extremely hard in the other. Having a commanded force in the normal
direction would allow the position of the robot in that direction to
become whatever is necessary to apply the force, and it will follow the
wall.</p>
<figure>
<iframe width="560" height="315" src="https://www.youtube.com/embed/PngdQGEUi7w?mute=1&rel=0" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen pdf="no"></iframe>
<p pdf="only"><a href="https://www.youtube.com/embed/PngdQGEUi7w">Click here to watch the video.</a></p>
<figcaption>A simple example of hybrid position/force control; the
robot needs to push down hard enough that the contact between the
fingers and book do not slip, but gentle enough that the contacts
between the book at the table <i>do</i> slide. This is naturally
accomplished with force control in the world $z$. But moving the
gripper/book in the $x,y$ plane is more naturally described using
position control. You can explore this example in more detail in <a
href="#hybrid_book_push">an exercise</a>.</figcaption>
</figure>
<todo>Check yourself: can the finger move the box? (only if the coefficient of friction is bigger).</todo>
<p>The choice of position control or force control need not be a binary
decision. We can simply apply both the position command (as in
stiffness/impedance control) and a "feed-forward" force command: $$u =
-mg + K_p (p^{F_d} - p^{F}) + K_d (v^{F_d} - v^{F}) -
f^{F}_{\text{feedforward}}.$$ Certainly this is only more general that
the explicit position-or-force mode in Eq. (\ref{eq:position-or-force});
we could achieve the explicit position-or-force in this interface by the
appropriate choices of $K_p$, $K_d$, and $f^F_{feedforward}.$ As we'll
see, this is quite similar to the interface provided by the iiwa (and
many other torque-controlled robots).</p>
</subsection>
</section>
<section><h1>The general case (using the manipulator equations)</h1>
<p>Using the floating finger/gripper is a good way to understand the main
concepts of force control without the details. But now it's time to
actually implement those strategies using joint commands that we can send to
the arm.</p>
<p>Our starting point is understanding that the equations of motion for a
fully-actuated robotic manipulator have a very structured form:
\begin{equation}M(q)\ddot{q} + C(q,\dot{q})\dot{q} = \tau_g(q) + u + \sum_i
J^T_i(q)f^{c_i}.\label{eq:manipulator} \end{equation} The left side of the
equation is just a generalization of "mass times acceleration", with the
mass matrix, $M$, and the Coriolis terms $C$. The right hand side is the
sum of the (generalized) forces, with $\tau_g(q)$ capturing the forces due
to gravity, $u$ the joint torques supplied by the motors, and $f^{c_i}$ is
the Cartesian force due to the $i$th contact, where $J_i(q)$ is the $i$th
"contact Jacobian". I introduced a version of these briefly when we
described multibody dynamics for dropping random objects into the bin, and
have more notes <a
href="http://underactuated.mit.edu/multibody.html">available here</a>. For
the purposes of the remainder of this chapter, we can assume that the robot
is bolted to the table, so does not have any floating joints; I've therefore
used $\dot{q}$ and $\ddot{q}$ instead of $v$ and $\dot{v}$.</p>
<subsection><h1>Trajectory tracking</h1>
<!--
<p>There is a variation on the joint-space stiffness control that we use
often in simulation; we call it <a
href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_inverse_dynamics_controller.html">InverseDynamicsControl</a>. </p>
-->
<p>Let us ignore the contact terms for a moment: $$M(q)\ddot{q} +
C(q,\dot{q})\dot{q} = \tau_g(q) + u.$$ The <i>forward dynamics</i>
problem, which we use during simulation, is to compute the
accelerations, $\ddot{q}$, given the joint torques, $u$, (and $q,
\dot{q}$). The <i>inverse dynamics</i>
problem, which we can use in control, is to compute $u$ given $\ddot{q}$
(and $q, \dot{q}$). As a system, it looks like</p>
<div>
<script type="text/javascript">
var inverse_dynamics = { "name": "InverseDynamics",
"input_ports" : ["estimated_state", "desired_acceleration"],
"output_ports" : ["actuation_input"]
};
document.write(system_html(inverse_dynamics, "https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_inverse_dynamics.html"));
</script>
</div>
<p>This system implements the controller: $$u = M(q)\ddot{q}_d +
C(q,\dot{q})\dot{q} - \tau_g(q),$$ so that the resulting dynamics are
$M(q)\ddot{q} = M(q)\ddot{q}_d.$ Since we know that $M(q)$ is invertible,
this implies that $\ddot{q} = \ddot{q}_d$.</p>
<p>When we put the contact forces back in, In practice, we only have
estimates of the dynamics (mass matrix, etc) and even the states $q$ and
$\dot{q}$. We will need to account for these errors when we determine
the acceleration commands to send. For example, if we wanted to follow a
desired joint trajectory, $q_0(t)$, we do not just differentiate the
trajectory twice and command $\ddot{q}_0(t)$, but we could instead
command e.g. $$\ddot{q}_d = \ddot{q}_0(t) + K_p(q_0(t) - q) +
K_d(\dot{q}_0(t) - \dot{q}).$$ One could also include an integral gain
term.</p>
<p>The resulting <code>System</code> has one additional input port for the desired state:</p>
<div>
<script type="text/javascript">
var inverse_dynamics = { "name": "InverseDynamicsController",
"input_ports" : ["estimated_state", "desired_state", "desired_acceleration"],
"output_ports" : ["actuation_input"]
};
document.write(system_html(inverse_dynamics, "https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1controllers_1_1_inverse_dynamics_controller.html"));
</script>
</div>
<p>This "inverse dynamics control" also appears in the literature with a
few other names, such as "computed-torque control" and even the
"feedforward-plus-feedback-linearizing control" <elib
part="11.4.1.3">Lynch17</elib>.</p>
<details style-type="box"><summary><b>Check yourself:</b> What is the
difference between the joint stiffness control and the inverse dynamics
control?</summary> <p>The closed-loop dynamics with the joint stiffness
control (taking $\tau_{ff} = 0$) are given by $$M(q)\ddot{q} +
C(q,\dot{q})\dot{q} + K_p(q - q_d) + K_d(\dot{q} - \dot{q}_d) =
\tau_{ext},$$ where $\tau_{ext}$ summarized the results of (unmodeled)
forces like contacts. The closed-loop dynamics with the inverse dynamics
controllers are given by $$\ddot{q} + K_p(q - q_d) + K_d(\dot{q} -
\dot{q}_d) = M^{-1}(q) \tau_{ext}.$$ So the inverse dynamics controller
writes the feedback law in the units of accelerations rather than forces;
this would be similar to shaping the internal matrix into the diagonal
matrix.</p>
<p>The ability to realize this cancellation in hardware, however, will be
limited by the accuracy with which we estimate the model parameters,
$\hat{M}(q)$ and $\hat{C}(q,\dot{q})$. The stiffness controller, on the
other hand, achieves much of the desired performance but only attempts to
cancel the gravity (and perhaps friction) terms; it does not shape the
inertia.</p></details>
</subsection>
<!--
<subsection><h1>Direct force control</h1>
<p>Let's bring back our contact forces, but assume that we are only making
contact at a single, known, point on the robot: $$M(q)\ddot{q} +
C(q,\dot{q})\dot{q} = \tau_g(q) + u + J^T(q)f^c.$$ <!--Very often in
manipulation we assume that our dynamics are <i>quasi-static</i>: that the
velocities and accelerations are small enough that the terms which depend
on them can be safely ignored. In a quasi-static analysis, we have the force-balance equations $$\tau_g(q) + u = - J^T(q) F^c.$$ As a result, we can command the desired force by compensating for gravity and any other known forces</p> --> <!--
</subsection>
-->
<subsection><h1>Joint stiffness control</h1>
<p>In practice, the way that we most often interface with the iiwa is
through the its "joint-impedance control" mode, which is written up
nicely in <elib>Ott08+Albu-Schaffer07</elib>. For our purposes, we can
view this as a stiffness control in joint space: $$u = -\tau_g(q) +
K_p(q_d - q) + K_d(\dot{q}_d - \dot{q}) + \tau_{ff},$$ where $K_p, K_d$
are positive diagonal matrices, and $\tau_{ff}$ is a "feed-forward"
torque. In practice the controller also includes some joint friction
compensation<elib>Albu-Schaffer01</elib>, but I've left those friction
terms out here for the sake of brevity. The controller does also do some
shaping of the inertias (earning it the label "impedance control" instead
of only "stiffness control"), but only the rotor inertias, and the user
does not set these effective inertias. From the user perspective it
should be viewed as stiffness control.</p>
<details style-type="box"><summary><b>Check yourself:</b> What is the
difference between traditional position control with a PD controller and
joint-stiffness control?</summary> <p>The difference in the algebra is
quite small. A PD control would typically have the form $$u=K_p(q_d-q) +
K_d(\dot{q}_d - \dot{q}),$$ whereas stiffness control is $$u = -\tau_g(q)
+ K_p(q_d-q) + K_d(\dot{q}_d - \dot{q}).$$ In other words, stiffness
control tries to cancel out the gravity and any other estimated terms,
like friction, in the model. As written, this obviously requires an
estimated model (which we have for iiwa, but don't believe we have for
most arms with large gear-ratio transmissions) and torque control. But
this small difference in the algebra can make a profound difference in
practice. The controller is no longer depending on error to achieve the
joint position in steady state. As such we can turn the gains way down,
and in practice have a much more compliant response while still achieving
good tracking when there are no external torques.</p></details>
<todo>Gravity comp is a classic demo of how well you model/control your robot. Show kuka videos.</todo>
<todo>C++ implementation. Add code example(s) here.</todo>
</subsection>
<subsection><h1>Cartesian stiffness control</h1>
<p>A better analogy for the control we were doing with the point finger
example is to write a controller so that the robot acts like a simple
dynamical system in the world frame. To do that, we have to identify a
frame, $E$ for "end-effector", on the robot where we want to impose these
simple dynamics -- ideally the origin of this frame will be the expected
point of contact with the environment. Following our treatment of <a
href="pick.html#kinematics">kinematics</a>
and <a href="pick.html#jacobian">differential kinematics</a>, we'll
define the forward kinematics of this frame as: \begin{equation}p^E =
f_{kin}(q), \qquad v^E = \dot{p}^E = J(q) \dot{q}, \qquad a^E =
\ddot{p}^E = J(q)\ddot{q} + \dot{J}(q)
\dot{q}.\label{eq:kinematics}\end{equation} We haven't actually written
the second derivative before, but it follows naturally from the chain
rule. Also, I've restricted this to the Cartesian positions for
simplicity; one can think about the orientation of the end-effector, but
this requires some care in defining the 3D stiffness in orientation (e.g.
<elib>Suh22</elib>).</p>
<p>One of the big ideas from manipulator control is that we can actually
write the dynamics of the robot in the frame $E$, by parameterizing the
joint torques as a translational force applied to body $B$ at $E$,
$f^{B_E}_u$: $u = J^T(q) f^{B_E}_u$. This comes from the <a
href="https://ocw.mit.edu/courses/mechanical-engineering/2-12-introduction-to-robotics-fall-2005/lecture-notes/chapter6.pdf">principle
of virtual work</a>. By substituting this and the manipulator equations
(\ref{eq:manipulator}) into (\ref{eq:kinematics}) and assuming that the
only external contacts happen at $E$, we can write the dynamics:
\begin{equation} M_E(q) \ddot{p}^E + C_E(q,\dot{q})\dot{q} = f^{B_E}_g(q)
+ f^{B_E}_u + f^{B_E}_{ext} \label{eq:cartesian_dynamics},\end{equation}
where $$M_E(q) = (J M^{-1} J^T)^{-1}, \qquad C_E(q,\dot{q}) = M_E \left(J
M^{-1} C+ \dot{J} \right), \qquad f^{B_E}_g(q) = M_E J M^{-1} \tau_g.$$
Now if we simply apply a controller analogous to what we used in joint
space, e.g.: $$f^{B_E}_u = -f^{B_E}_g(q) + K_p(p^{E_d} - p^E) +
K_d(\dot{p}^{E_d} - \dot{p}^E),$$ then we can achieve the desired
closed-loop dynamics <i>at the end-effector</i>: $$M_E(q) \ddot{p}^E +
C_E(q,\dot{q})\dot{q} + K_p(p^{E} - p^{E_d}) + K_d(\dot{p}^{E} -
\dot{p}^{E_d}) = f^{B_E}_{ext}.$$ So if I push on the robot at the
end-effector, the entire robot will move in a way that will make it feel
like I'm pushing on a spring-damper system. Beautiful!</p>
<p>When $J(q)$ can be rank-deficient (for instance, if you have more
degrees of freedom than you are trying to control at the end-effector),
you'll also want to add some terms to similar to
<a href="pick.html#joint_centering">stabilize the null space of your
Jacobian</a>. Since we are operating here with torques instead of
velocity commands, the natural analogue is to write the a joint-centering
PD controller that operates in the nullspace of the end-effector control:
$$u = J^Tf^{B_E}_u + [I - J^+J][K_{p,joint} (q_0 - q) -
K_{d,joint}\dot{q}].$$</p>
<p>This approach of programming the task-space dynamics with
secondary joint-space objectives operating in the nullspace was developed
in <elib>Khatib87</elib> and is known as <i>operational space control</i>. It can be generalized to a rich composition of
<i>prioritized</i> task-space and joint-space control objectives; the
prioritization is accomplished by putting lower-priority tasks in the
nullspace of the primary task. Like the joint-centering differential
inverse kinematics, these can also be understood through the lens of
least-squares optimization.</p>
</subsection>
<subsection><h1>Some implementation details on the iiwa</h1>
<p>The implementation of the low-level impedance controllers has many
details that are explained nicely in <elib>Ott08+Albu-Schaffer07</elib>.
In particular, the authors go to quite some length to implement the
impedance law in the actuator coordinates rather than the joint
coordinates (remember that they have an elastic transmission in between
the two). I suspect there are many subtle reasons to prefer this, but
they go to lengths to demonstrate that they can make a true passivity
argument with this controller.</p>
<p>The iiwa interface offers a Cartesian impedance control mode. If we
want high performance stiffness control in end-effector coordinates, then
we should definitely use it! The iiwa controller runs at a much higher
bandwidth (faster update rate) than the interface we have over the
provided network API, and many implementation details that they have gone
to great lengths to get right. But in practice we do not use it, because
we cannot convert between joint impedance control and Cartesian impedance
control without shutting down the robot. Sigh. In fact we cannot even
change the stiffness gains nor the frame $C$ (aka the "end-effector
location") without stopping the robot. So we stay in joint impedance mode
and command some Cartesian forces through $\tau_{ff}$ if we desire them.
(If you are interested in the driver details, then I would recommend the
documentation for the
<a href="https://frankaemika.github.io/docs/libfranka.html">Franka Control
Interface</a> which is much easier to find and read, and is very similar
to functionality provided by the iiwa driver.)</p>
<p>You might also notice that the interface we provide to the
<code>IiwaDriver</code> in the <code>HardwareStation</code> takes a
desired joint position for the robot, but not a desired joint velocity.
That is because we cannot actually send a desired joint velocity to the
iiwa controller. In practice, we believe that they are numerically
differentiating our position commands to obtain the desired velocity,
which adds a delay of a few control timesteps (and sometimes <a
href="https://github.com/RobotLocomotion/drake/issues/11955#issuecomment-524467883">non-monotonic
behavior</a>). I believe that the reason why we aren't allowed to send
our own joint velocity commands is to to make the passivity argument in
their paper go through, and perhaps to make a simpler/safer API -- they
don't want users to be able to send a series of inconsistent position and
velocity commands (where the velocities are not actually the time
derivatives of the positions).</p>
<todo>Summarize the email exchange with the kuka folks describing these
details.</todo>
<p>Since the controller attempts to cancel gravitational terms, one can
also tell the iiwa firmware about the lumped inertia of the gripper. This
is set just once (in the pendant), and cannot be changed while the robot
is moving; if you pick something up (or even move the fingers), those
changes in the gravitational terms will not be compensated in the
controller.</p>
<p>Although the JointStiffnessController in Drake is the best model for
the iiwa control stack, we commonly use the InverseDynamicsController in
our simulations instead. This is for a fairly subtle reason -- the
numerics are better. The effective stiffness of the differential
equations to achieve comparable stiffness in the physics is smaller, and
one can take bigger integration timesteps. On the iiwa hardware, we
commonly use [800., 600, 600, 600, 400, 200, 200] Nm/rad as the stiffness
values for the JointStiffnessController; but simulating with those
requires small integration steps.</p>
</subsection>
<todo>Admittance control</todo>
<todo>Adaptive control. Slotine WAM throwing.</todo>
<todo>Estimating contact location. contact particle filter.
haddadin videos (e.g. http://www.diag.uniroma1.it/~deluca/IIT_Seminar_Jan23_2019_ADL.pdf)
De Luca pHRI
https://www.youtube.com/playlist?list=PLvAUmIzqq6oaRtwX9l9sjDhcNMXNCGSN0</todo>
</section>
<section><h1>Putting it all together</h1>
<todo>Flip up the box using cartesian stiffness control.</todo>
</section>
<section><h1>Peg in hole</h1>
<p>One of the classic problems for manipulator force control, inspired by
robotic assembly, is the problem of mating two parts together under
tight kinematic tolerances. Perhaps the cleanest and most famous version of
this is the problem of inserting a (round) peg into a (round) hole. If your
<a href="https://en.wikipedia.org/wiki/Square_peg_in_a_round_hole">peg is
square and your hole is round</a>, I can't help. Interestingly, much of the
literature on this topic in the late 1970's and early 1980's came out of MIT
and the Draper Laboratory.</p>
<figure><img width="35%" src="data/peg-in-hole.png"> <img style="margin-left:4%; margin-right:10%;" width="18%" src="data/peg-in-hole-force.png"> <img width="16%" src="data/peg-in-hole-moment.png"><figcaption>(Left) the
"peg in hole" insertion problem with kinematic tolerance $c$. (Middle) the
desired response to a contact force. (Right) the desired response to a
contact moment. Figures reproduced (with some symbols removed) from
<elib>Drake78</elib>.
</figcaption></figure>
<p>For many peg insertion tasks, it's acceptable to add a small <a
href="https://en.wikipedia.org/wiki/Chamfer_(geometry)">chamfer</a> to the
part and/or the top of the hole. This allows small errors in horizontal
alignment to be accounted for with a simple compliance in the horizontal
direction. Misalignment in orientation is even more interesting. Small
misalignments can cause the object to become "jammed", at which point the
frictional forces become large and ambiguous (under the Coulomb model); we
really want to avoid this. Again, we can avoid jamming by allowing a
rotational compliance at the contact. I think you can see why this is a
great example for force control!</p>
<p>The key insight here is that the rotational stiffness that we want
should result in rotations around the part/contact, not around the gripper.
We can program this with stiffness control; but for very tight tolerances
the sensing and bandwidth requirements become a real limitation. One of
the coolest results in force control is the realization that a clever
physical mechanism can be used to produce a "remote-centered compliance"
(RCC) completely mechanically, with infinite bandwidth and no sensing
required <elib>Drake78</elib>!</p>
<figure><img width="40%" src="data/passive_remote_center_compliance.png">
<img style="margin-left:4%; margin-bottom:3%" width="55%"
src="data/Remote_Center_of_Compliance.svg"><figcaption>A passive
remote-center compliance device. (Left) concept schematic from
<elib>Drake78</elib>. (Right) schematic of a mechanical implementation;
image credit for <a
href="https://en.wikipedia.org/wiki/Remote_Center_Compliance">Arne
Nordmann, 2008</a>. See <a href="https://www.ati-ia.com/products/compliance/Compensator_main.aspx">here</a> for some example products, and <a href="https://www.youtube.com/watch?v=qFTUFHGGZIg">here</a> for a nice (older) video.</figcaption></figure>