forked from RussTedrake/underactuated
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathpend.html
More file actions
793 lines (625 loc) · 44.4 KB
/
Copy pathpend.html
File metadata and controls
793 lines (625 loc) · 44.4 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
<!DOCTYPE html>
<html>
<head>
<title>Underactuated Robotics: The Simple Pendulum</title>
<meta name="Underactuated Robotics: The Simple Pendulum" content="text/html; charset=utf-8;" />
<link rel="canonical" href="http://underactuated.mit.edu/pend.html" />
<script src="https://hypothes.is/embed.js" async></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="htmlbook/MathJax/es5/tex-chtml.js">
</script>
<script>window.MathJax || document.write('<script type="text/javascript" src="https://cdn.jsdelivr.net/npm/mathjax@3/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('underactuated');">
<div data-type="titlepage">
<header>
<h1><a href="index.html" style="text-decoration:none;">Underactuated Robotics</a></h1>
<p data-type="subtitle">Algorithms for Walking, Running, Swimming, Flying, and Manipulation</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, 2021<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><b>Note:</b> These are working notes used for <a
href="http://underactuated.csail.mit.edu/Spring2021/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2021 semester. <a
href="https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg">Lecture videos are available on YouTube</a>.</p>
<table style="width:100%;"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=intro.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=acrobot.html>Next Chapter</a></td>
</tr></table>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 1">
<h1>The Simple Pendulum</h1>
<a style="float:right; margin-top:-80px;" target="pend" href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/pend.ipynb"><img src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open Corresponding Notebook In Colab"/></a>
<div style="clear:right;"></div>
<section><h1>Introduction</h1>
<p> Our goals for this chapter are modest: we'd like to understand the
dynamics of a pendulum.</p>
<p>Why a pendulum? In part, because the dynamics of a majority of our
multi-link robotics manipulators are simply the dynamics of a large number
of coupled pendula. Also, the dynamics of a single pendulum are rich enough
to introduce most of the concepts from nonlinear dynamics that we will use
in this text, but tractable enough for us to (mostly) understand in the next
few pages. </p>
<figure> <img width="30%" src="figures/simple_pend.svg"/>
<figcaption>The simple pendulum</figcaption> </figure>
<p> The Lagrangian derivation of the equations of motion (as described in
the appendix) of the simple pendulum yields: \begin{equation*} m l^2
\ddot\theta(t) + mgl\sin{\theta(t)} = Q. \end{equation*} We'll consider the
case where the generalized force, $Q$, models a damping torque (from
friction) plus a control torque input, $u(t)$: $$Q = -b\dot\theta(t) +
u(t).$$ </p>
</section>
<section class="drake"><h1>Nonlinear dynamics with a
constant
torque</h1>
<p> Let us first consider the dynamics of the pendulum if it is driven in a
particular simple way: a torque which does not vary with time:
\begin{equation} ml^2 \ddot\theta + b\dot\theta + mgl \sin\theta = u_0.
\end{equation} </p>.
<example><h1>Simple Pendulum in Python</h1>
<p>You can experiment with this system in <drake></drake> using</p>
<p><a target="pend"
href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/pend.ipynb#scrollTo=C1sdq2R88C16"><img
src="https://colab.research.google.com/assets/colab-badge.svg"
alt="Open In Colab"/></a>
</p>
</example>
<p> These are relatively simple differential equations, so if I give you
$\theta(0)$ and $\dot\theta(0)$, then you should be able to integrate them
to obtain $\theta(t)$... right? Although it is possible, integrating even
the simplest case ($b = u = 0$) involves elliptic integrals of the first
kind; there is relatively little intuition to be gained here. </p>
<p> This is in stark contrast to the case of linear systems, where much of
our understanding comes from being able to explicitly integrate the
equations. For instance, for a simple linear system we have $$\dot{q} = a q
\quad \rightarrow \quad q(t) = q(0) e^{at},$$ and we can immediately
understand that the long-term behavior of the system is a (stable) decaying
exponential if $a<0$, an (unstable) growing exponential if $a>0$, and that
the system does nothing if $a=0$. Here we are with certainly one of the
simplest nonlinear systems we can imagine, and we can't even solve this
system? </p>
<p> All is not lost. If what we care about is the long-term behavior of the
system, then there are a number of techniques we can apply. In this
chapter, we will start by investigating graphical solution methods. These
methods are described beautifully in a book by Steve
Strogatz<elib>Strogatz94</elib>.</p>
<subsection><h1>The overdamped pendulum</h1>
<p> Let's start by studying a special case -- intuitively when
$b\dot\theta \gg ml^2\ddot\theta$ -- which via <a
href="https://en.wikipedia.org/wiki/Dimensionless_quantity">dimensional
analysis</a> (using the natural frequency $\sqrt{\frac{g}{l}}$ to match
units) occurs when $b \sqrt\frac{l}{g} \gg ml^2$. This is the case of
heavy damping, for instance if the pendulum was moving in molasses. In
this case, the damping term dominates the acceleration term, and we have:
$$ml^2 \ddot\theta + b\dot\theta \approx b\dot\theta = u_0 -
mgl\sin\theta.$$ In other words, in the case of heavy damping, the system
looks approximately first order. This is a general property of
heavily-damped systems, such as fluids at very low Reynolds number. </p>
<p> I'd like to ignore one detail for a moment: the fact that $\theta$
wraps around on itself every $2\pi$. To be clear, let's write the system
without the wrap-around as: \begin{equation}b\dot{x} = u_0 -
mgl\sin{x}.\label{eq:overdamped_pend_ct}\end{equation} Our goal is to
understand the long-term behavior of this system: to find $x(\infty)$
given $x(0)$. Let's start by plotting $\dot{x}$ vs $x$ for the case when
$u_0=0$:</p>
<figure> <img width="70%" src="figures/pend_sinx.svg"/> </figure>
<p> The first thing to notice is that the system has a number of <em>fixed
points</em> or <em>steady states</em>, which occur whenever $\dot{x} = 0$.
In this simple example, the zero-crossings are $x^* = \{..., -\pi, 0, \pi,
2\pi, ...\}$. When the system is in one of these states, it will never
leave that state. If the initial conditions are at a fixed point, we know
that $x(\infty)$ will be at the same fixed point.</p>
<p>Next let's investigate the behavior of the system in the local vicinity
of the fixed points. Examining the fixed point at $x^* = \pi$, if the
system starts just to the right of the fixed point, then $\dot{x}$ is
positive, so the system will move away from the fixed point. If it starts
to the left, then $\dot{x}$ is negative, and the system will move away in
the opposite direction. We'll call fixed-points which have this property
<em>unstable</em>. If we look at the fixed point at $x^* = 0$, then the
story is different: trajectories starting to the right or to the left will
move back towards the fixed point. We will call this fixed point
<em>locally stable</em>. More specifically, we'll distinguish between
multiple types of stability (where $\epsilon$ is used to denote an arbitrary <em>small</em> scalar quantity):
<ul>
<li>Locally <b>stable</b> in the sense of Lyapunov (i.s.L.). A fixed
point, $x^*$ is locally stable i.s.L. if for every $\epsilon > 0$,
I can produce a $\delta > 0$ such that if $\| x(0) - x^* \| < \delta$
then $\forall t$ $\| x(t) - x^*\| < \epsilon$. In words, this means
that for any ball of size $\epsilon$ around the fixed point, I can
create a ball of size $\delta$ which guarantees that if the system is
started inside the $\delta$ ball then it will remain inside the
$\epsilon$ ball for all of time.</li>
<li>Locally <b>attractive</b>. A fixed point is locally attractive if
$x(0) = x^* + \epsilon$ implies that $\lim_{t\rightarrow \infty} x(t) =
x^*$.</li>
<li>Locally <b>asymptotically stable</b>. A fixed point is locally
asymptotically stable if it is locally stable i.s.L. and locally attractive.</li>
<li>Locally <b>exponentially stable</b>. A fixed point is locally
exponentially stable if $x(0) = x^* + \epsilon$ implies that $\| x(t) -
x^* \| < Ce^{-\alpha t}$, for some positive constants $C$ and
$\alpha$.</li>
<li><b>Unstable</b>. A fixed point is unstable if it is not locally stable i.s.L.</li>
</ul>
<p>An initial condition near a fixed point that is stable in the sense of
Lyapunov may never reach the fixed point (but it won't diverge), near an
asymptotically stable fixed point will reach the fixed point as $t
\rightarrow \infty$, and near an exponentially stable fixed point will
reach the fixed point with a bounded rate. An exponentially stable fixed
point is also an asymptotically stable fixed point, but the converse is
not true. Attractivity does not actually imply Lyapunov
stability†<sidenote>† we can't see that in one dimension so
will have to hold that example for a moment</sidenote>, which is why we
requires i.s.L. specifically for the definition of asymptotic stability.
Systems which are stable i.s.L. but not asymptotically stable are easy to
construct (e.g. $\dot{x} = 0$). Interestingly, it is also possible to have
nonlinear systems that converge (or diverge) in finite-time; a so-called
<em>finite-time stability</em>; we will see examples of this later in the
book, but it is a difficult topic to penetrate with graphical analysis.
Rigorous nonlinear system analysis is rich with subtleties and surprises.
Moreover, these differences actually matter -- the code that we will write
to stabilize the systems will be subtly different depending on what type
of stability we want, and it can make or break the success of our
methods.</p>
<p> Our graph of $\dot{x}$ vs. $x$ can be used to convince ourselves of
i.s.L. and asymptotic stability by visually inspecting $\dot{x}$ in the
vicinity of a fixed point. Even exponential stability can be inferred if
we can find a negatively-sloped line passing through the equilibrium point
which separates the graph of $f(x)$ from the horizontal axis, since it
implies that the nonlinear system will converge at least as fast as the
linear system represented by the straight line. I will graphically
illustrate unstable fixed points with open circles and stable fixed points
(i.s.L.) with filled circles. </p>
<p>Next, we need to consider what happens to initial conditions which
begin farther from the fixed points. If we think of the dynamics of the
system as a flow on the $x$-axis, then we know that anytime $\dot{x} > 0$,
the flow is moving to the right, and $\dot{x} < 0$, the flow is moving to
the left. If we further annotate our graph with arrows indicating the
direction of the flow, then the entire (long-term) system behavior becomes
clear:</p>
<figure> <img width="70%" src="figures/pend_sinx_annotated.svg"/>
</figure>
<p> For instance, we can see that any initial condition $x(0) \in
(-\pi,\pi)$ will result in $\lim_{t\rightarrow \infty} x(t) = 0$. This
region is called the <em>basin of attraction</em> of the fixed point at
$x^* = 0$. Basins of attraction of two fixed points cannot overlap, and
the manifold separating two basins of attraction is called the
<em>separatrix</em>. Here the unstable fixed points, at $x^* = \{..,
-\pi, \pi, 3\pi, ...\}$ form the separatrix between the basins of
attraction of the stable fixed points. </p>
<p> As these plots demonstrate, the behavior of a first-order one
dimensional system on a line is relatively constrained. The system will
either monotonically approach a fixed-point or monotonically move toward
$\pm \infty$. There are no other possibilities. Oscillations, for
example, are impossible. Graphical analysis is a fantastic analysis tool
for many first-order nonlinear systems (not just pendula); as illustrated
by the following example: </p>
<example><h1>Nonlinear autapse</h1>
<p></p>Consider the following system: \begin{equation} \dot{x} + x =
\tanh(w x), \end{equation} which is plotted below for two values of $w$.
It's convenient to note that $\tanh(z) \approx z$ for small $z$. For $w
\le 1$ the system has only a single fixed point. For $w > 1$ the system
has three fixed points : two stable and one unstable.</p>
<figure> <img width="80%" src="figures/pend_autapse.svg"/>
</figure>
<p>These equations are not arbitrary - they are actually a model for one
of the simplest neural networks, and one of the simplest model of
persistent memory<elib>Seung00</elib>. In the equation $x$ models the
firing rate of a single neuron, which has a feedback connection to
itself. $\tanh$ is the activation (sigmoidal) function of the neuron,
and $w$ is the weight of the synaptic feedback.</p>
<p>Experiment with it for yourself:</p>
<p><a target="pend"
href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/pend.ipynb#scrollTo=zKmlDpfeKUSo"><img
src="https://colab.research.google.com/assets/colab-badge.svg"
alt="Open In Colab"/></a>
</p>
<p>As a bonus, you'll also find a the equations of an <a
href="https://en.wikipedia.org/wiki/Long_short-term_memory">LSTM</a>
unit that you can also experiment with. See if you can figure it out!
</p>
</example>
<p> One last piece of terminology. In the neuron example, and in many
dynamical systems, the dynamics were parameterized; in this case by a
single parameter, $w$. As we varied $w$, the fixed points of the system
moved around. In fact, if we increase $w$ through $w=1$, something
dramatic happens - the system goes from having one fixed point to having
three fixed points. This is called a <em>bifurcation</em>. This
particular bifurcation is called a pitchfork bifurcation. We often draw
bifurcation diagrams which plot the fixed points of the system as a
function of the parameters, with solid lines indicating stable fixed
points and dashed lines indicating unstable fixed points, as seen in the
figure:</p>
<figure> <todo>bifurcation diagram asymptotes to $x^* = 1$</todo> <img
width="80%" src="figures/pend_autapse_bifurcation.svg"/>
<figcaption>Bifurcation diagram of the nonlinear autapse.</figcaption>
</figure>
<p> Our pendulum equations also have a (saddle-node) bifurcation when we
change the constant torque input, $u_0$. <!-- This is the subject of
exercise pend_bifurcation.--> Finally, let's return to the
original equations in $\theta$, instead of in $x$. Only one point to
make: because of the wrap-around, this system will <em>appear</em> to have
oscillations. In fact, the graphical analysis reveals that the pendulum
will turn forever whenever $|u_0| > mgl$, but now you understand
that this is not an oscillation, but an instability with $\theta
\rightarrow \pm \infty$. </p>
<p>You can find another beautiful example of these concepts (fixed points, basins of attraction, bifurcations) from recurrent neural networks in the exercise below on <a href="#hopfield">Hopfield networks</a>.</p>
</subsection> <!-- end of overdamped pend -->
<subsection id="pend_zero_torque"><h1>The undamped pendulum with zero torque</h1>
<p> Consider again the system $$ml^2 \ddot\theta = u_0 - mgl \sin\theta -
b\dot\theta,$$ this time with $b = 0$. This time the system dynamics are
truly second-order. We can always think of any second-order system as
(coupled) first-order system with twice as many variables. Consider a
general, autonomous (not dependent on time), second-order system,
$$\ddot{q} = f(q,\dot q,u).$$ This system is equivalent to the
two-dimensional first-order system \begin{align*} \dot x_1 =& x_2 \\ \dot
x_2 =& f(x_1,x_2,u), \end{align*} where $x_1 = q$ and $x_2 = \dot q$.
Therefore, the graphical depiction of this system is not a line, but a
vector field where the vectors $[\dot x_1, \dot x_2]^T$ are plotted over
the domain $(x_1,x_2)$. This vector field is known as the <em>phase
portrait</em> of the system.</p>
<p> In this section we restrict ourselves to the simplest case when $u_0 =
0$. Let's sketch the phase portrait. First sketch along the
$\theta$-axis. The $x$-component of the vector field here is zero, the
$y$-component is $-\frac{g}{l}\sin\theta.$ As expected, we have fixed
points at $\pm \pi, ...$ Now sketch the rest of the vector field. Can you
tell me which fixed points are stable? Some of them are stable i.s.L.,
none are asymptotically stable.</p>
<figure> <img width="80%" src="figures/pend_undamped_phase.svg"/>
</figure>
<subsubsection><h1>Orbit calculations</h1>
<p> You might wonder how we drew the black contour lines in the figure
above. We could have obtained them by simulating the system
numerically, but those lines can be easily obtained in closed-form.
Directly integrating the equations of motion is difficult, but at least
for the case when $u_0 = 0$, we have some additional physical insight
for this problem that we can take advantage of. The kinetic energy,
$T$, and potential energy, $U$, of the pendulum are given by $$T =
\frac{1}{2}I\dot\theta^2, \quad U = -mgl\cos(\theta),$$ where $I=ml^2$
and the total energy is $E(\theta,\dot\theta) =
T(\dot\theta)+U(\theta)$. The undamped pendulum is a conservative
system: total energy is a constant over system trajectories. Using
conservation of energy, we have:
\begin{gather*} E(\theta(t),\dot\theta(t)) = E(\theta(0),\dot\theta(0))
= E_0 \\ \frac{1}{2} I \dot\theta^2(t) - mgl\cos(\theta(t)) = E_0 \\
\dot\theta(t) = \pm \sqrt{\frac{2}{I}\left[E_0 +
mgl\cos\left(\theta(t)\right)\right]} \end{gather*}
Using this, if you tell me $\theta$ I can determine one of two possible
values for $\dot\theta$, and the solution has all of the richness of the
black contour lines from the plot. This equation has a real solution
when $\cos(\theta) > \cos(\theta_{max})$, where $$\theta_{max} =
\begin{cases} \cos^{-1}\left( -\frac{E_0}{mgl} \right), & E_0 < mgl \\
\pi, & \text{otherwise}. \end{cases}$$ Of course this is just the
intuitive notion that the pendulum will not swing above the height where
the total energy equals the potential energy. As an exercise, you can
verify that differentiating this equation with respect to time indeed
results in the equations of motion.</p>
<p>The particular orbit defined by $E = mgl$ is special -- this is the orbit that visits the (unstable) equilibrium at the upright. It is known as the <a href="https://en.wikipedia.org/wiki/Homoclinic_orbit">homoclinic orbit</a>.</p>
</subsubsection>
<!-- these are correct, but take away from the flow. move to an appendix?
<subsubsection><h1>Trajectory calculations</h1>
<p> For completeness, I'll include what it would take to solve for $\theta(t)$,
even thought it cannot be accomplished using elementary functions. Feel free to
skip this subsection. We begin the integration with
\begin{gather*} \frac{d\theta}{dt} = \sqrt{\frac{2}{I}\left[E +
mgl\cos\left(\theta(t)\right)\right]} \\ \int_{\theta(0)}^{\theta(t)}
\frac{d\theta}{\sqrt{\frac{2}{I}\left[E +
mgl\cos\left(\theta(t)\right)\right]}} = \int_0^t dt' = t \end{gather*}
The
integral on the left side of this equation is an (incomplete) elliptic integral
of the first kind. Using the identity: $$\cos(\theta) = 1 - 2
\sin^2(\frac{1}{2}\theta),$$ and manipulating, we have $$t =
\sqrt{\frac{I}{2(E+mgl)}} \int_{\theta(0)}^{\theta(t)} \frac{d\theta}{\sqrt{1 -
k_1^2\sin^2(\frac{\theta}{2})}}, \quad \text{with
}k_1=\sqrt{\frac{2mgl}{E+mgl}}.$$ In terms of the incomplete elliptic integral
function, $$F(\phi,k) = \int_0^\phi \frac{d\theta}{\sqrt{1-k^2\sin^2\theta}},$$
accomplished by a change of variables. If $E <= mgl$, which is the case of
closed-orbits, we use the following change of variables to ensure $ 0 < k < 1 $ :
\begin{gather*}\phi = \sin^{-1}\left[ k_1 \sin\left( \frac{\theta}{2} \right)
\right] \\ \cos(\phi) d\phi = \frac{1}{2} k_1 \cos\left(\frac{\theta}{2}\right)
d\theta = \frac{1}{2} k_1 \sqrt{1 - \frac{\sin^2 (\phi)}{k_1^2}} d\theta
\end{gather*} so we have
\begin{gather*}
t = \frac{1}{k_1}\sqrt{\frac{2I}{(E+mgl)}} \int_{\phi(0)}^{\phi(t)}
\frac{d\phi}{\sqrt{1 - \sin^2(\phi)}} \frac{\cos(\phi)}{\sqrt{1 -
\frac{\sin^2\phi}{k_1^2}}} \\ = \sqrt{\frac{I}{mgl}} \left[
F\left(\phi(t),k_2\right) - F\left(\phi(0),k_2\right) \right],\quad
k_2 = \frac{1}{k_1}.\end{gather*} The inverse of $F$ is given by the
Jacobi elliptic functions (sn,cn,...), yielding:
<!-- http://en.wikipedia.org/wiki/Jacobi%27s_elliptic_functions -->
<!--
\begin{gather*}\sin(\phi(t)) = \text{sn}\left(t
\sqrt{\frac{mgl}{I}} + F\left(\phi(0),k_2\right),k_2 \right) \\
\theta(t) = 2\sin^{-1} \left[ k_2 \text{sn}\left(t
\sqrt{\frac{mgl}{I}} + F\left(\phi(0),k_2\right),k_2 \right) \right]
\end{gather*}
The function $\text{sn}$ used here can be evaluated in MATLAB by
calling $$\text{sn}(u,k) = \text{ellipj}(u,k^2).$$ The function
$F$ is not implemented in MATLAB, but implementations can be
downloaded. (note that $F(0,k) = 0$).</p>
<p>
For the open-orbit case, $E>mgl$, we use $$\phi =
\frac{\theta}{2},\quad \frac{d\phi}{d\theta} = \frac{1}{2},$$ yielding
\begin{gather*}
t = \frac{2I}{E+mgl} \int_{\phi(0)}^{\phi(t)} \frac{d\phi}{\sqrt{1 -
k_1^2 \sin^2(\phi)}} \\
\theta(t) = 2 \tan^{-1} \left[ \frac{ \text{sn} \left( t
\sqrt{\frac{E+mgl}{2I}} + F\left( \frac{\theta(0)}{2}, k_1 \right)
\right) } { \text{cn} \left( t
\sqrt{\frac{E+mgl}{2I}} + F\left( \frac{\theta(0)}{2}, k_1 \right)
\right) }
\right]
\end{gather*}
Notes: Use MATLAB's <code>atan2</code> and <code>unwrap</code> to recover the
complete trajectory.</p>
<!-- primary refs:
http://en.wikipedia.org/wiki/Pendulum_%28mathematics%29
http://kr.cs.ait.ac.th/~radok/math/mat6/calc81.htm
http://books.google.com/books?id=xWrJlTIYl_IC&pg=PA280&lpg=PA280&dq=pendulum+elliptic+integrals&source=web&ots=zas9k5qVc0&sig=Tz-OwdqS84VPM2I_pZbtaspVZNk#PPA280,M1 (Computational Physics: Problem Solving with Computers by Rubin H. Landau, Cristian C. Bordeianu, p.280)
http://en.wikipedia.org/wiki/Binomial_theorem
http://mathworld.wolfram.com/EllipticIntegraloftheFirstKind.html -->
<!--</section> -->
</subsection> <!-- end of undamped pend -->
<subsection><h1>The undamped pendulum with a constant
torque</h1>
<p> Now what happens if we add a constant torque? If you visualize the
bifurcation diagram, you'll see that the fixed points come together,
towards $q = \frac{\pi}{2}, \frac{5\pi}{2}, ...$, until they disappear.
One fixed-point is unstable, and one is stable.</p>
<!-- todo: find a way to add an animation or something in here. do a
python demo in class -->
</section> <!-- undamped constant torque -->
<p>Before we continue, let me now give you the promised example of a system
that is not stable i.s.L., but which attracts all trajectories as time
goes to infinity. We can accomplish this with a very pendulum-like example
(written here in polar coordinates):</p>
<example><h1>Unstable equilibrium point that attracts all trajectories</h1>
<p>Consider the system \begin{align*} \dot{r} &= r(1-r), \\ \dot\theta &= \sin^2
\left( \frac{\theta}{2} \right).\end{align*} This system has two equilibrium points, one at
$r^*=0,\theta^*=0$, and the other at $r^* = 1,\theta^*=0$. The fixed
point at zero is clearly unstable. The fixed point with $r^*=1$ attracts
all other trajectories, but it is not stable by any of our
definitions.</p>
</example>
<p>Take a minute to draw the vector field of this (you can draw each
coordinate independently, if it helps) to make sure you understand. Note
that to wrap-around rotation is convenient but not essential -- we could
have written the same dynamical system in cartesian coordinates without
wrapping. And if this still feels too arbitrary, we will see it happen in
practice when we introduce the energy-shaping swing-up controller for the
pendulum in the next chapter.</p>
<subsection><h1>The damped pendulum</h1>
<p>Now let's add damping back. You can still add torque to move the
fixed points (in the same way).</p>
<figure>
<img width="90%" src="figures/pend_damped_phase.svg"/>
<figcaption>Phase diagram for the damped pendulum</figcaption>
</figure>
<p>With damping, the downright fixed point of the pendulum now becomes
asymptotically stable (in addition to stable i.s.L.). Is it also
exponentially stable? How can we tell? One technique is to linearize the
system at the fixed point. A smooth, time-invariant, nonlinear system
that is locally exponentially stable <i>must</i> have a stable
linearization; we'll discuss linearization more in the next chapter. </p>
<p>Here's a thought exercise. If $u$ is no longer a constant, but a
function $\pi(\theta,\dot{\theta})$, then how would you choose $\pi$ to
stabilize the vertical position. Feedback cancellation is the trivial
solution, for example: $$u = \pi(\theta,\dot{\theta}) = 2mgl\sin\theta.$$
But these plots we've been making tell a different story. How would you
shape the natural dynamics - at each point pick a $u$ from the stack of
phase plots - to stabilize the vertical fixed point <em>with minimal
torque effort</em>? This is exactly the way that I would like you to
think about control system design. And we'll give you your first solution
techniques -- using dynamic programming -- in the next lecture.</p>
</subsection> <!-- damped pend -->
</section> <!-- constant torque -->
<section><h1>The torque-limited simple pendulum</h1>
<p>The simple pendulum is fully actuated. Given enough torque, we can
produce any number of control solutions to stabilize the originally
unstable fixed point at the top (such as designing a feedback controller to
effectively invert gravity).</p>
<p>The problem begins to get interesting (a.k.a. becomes underactuated)
if we impose a torque-limit constraint, $|u|\le u_{max}$. Looking at the
phase portraits again, you can now visualize the control problem. Via
feedback, you are allowed to change the direction of the vector field at
each point, but only by a fixed amount. Clearly, if the maximum torque is
small (smaller than $mgl$), then there are some states which cannot be
driven directly to the goal, but must pump up energy to reach the goal.
Furthermore, if the torque-limit is too severe and the system has damping,
then it may be impossible to swing up to the top. The existence of a
solution, and number of pumps required to reach the top, is a non-trivial
function of the initial conditions and the torque-limits.</p>
<p>Although this system is very simple, obtaining a solution with
general-purpose algorithms requires much of the same reasoning necessary for
controlling much more complex underactuated systems; this problem will be a
work-horse for us as we introduce new algorithms throughout this book.</p>
<subsection id="energy_shaping"><h1>Energy-shaping control</h1>
<p>In the specific case of the pendulum, we can give a satisfying
hand-designed nonlinear controller based on our intuition of pumping
energy into the system. We have already observed that the total energy of
the pendulum is given by $$E = \frac{1}{2} m l^2 \dot\theta^2 -
mgl\cos\theta.$$ To understand how to control the energy, let us examine
how that energy changes with time: \begin{align*} \dot{E} =&
ml^2\dot\theta \ddot\theta + \dot\theta mgl\sin\theta \\ =& \dot\theta
\left[ u - mgl\sin\theta \right] + \dot\theta mgl\sin\theta \\ =&
u\dot\theta. \end{align*} In words, adding energy to the system is simple
- apply torque in the same direction as $\dot\theta$. To remove
energy, apply torque in the opposite direction (e.g., damping).</p>
<p>To swing up the pendulum, even with torque limits, let us use this
observation to drive the system to its homoclinic orbit, and then let the
dynamics of the pendulum carry us to the upright equilibrium. Recall that
the homoclinic orbit has energy $mgl$ -- let's call this our
<i>desired</i> energy:$$E^d = mgl.$$ Furthermore, let's define the
difference between our current energy and this desired energy as
$\tilde{E} = E - E^d$, and note that we still have $$\dot{\tilde{E}} =
\dot{E} = u\dot\theta.$$ Now consider the feedback controller of the form
$$u = -k \dot\theta \tilde{E},\quad k>0.$$ I admit that this looks a bit
strange; it was chosen for the simple reason that it turns the resulting
"error dynamics" into something simple: $$\dot{\tilde{E}} = - k
\dot\theta^2 \tilde{E}.$$ Think about the graphical analysis of this
system if you were to draw $\dot{\tilde{E}}$ vs. $\tilde{E}$ for any fixed
$\dot\theta$ -- it's a straight line separated from the horizontal axis
which would imply an exponential convergence: $\tilde{E} \rightarrow 0.$
This is true for any $\dot\theta$, except for $\dot\theta=0$ (so it will
not actually swing us up from the downright fixed point... but if you
nudge the system just a bit, then it will start pumping energy and will
swing all of the way up). The essential property is that when $E > E^d$,
we should remove energy from the system (damping) and when $E < E^d$, we
should add energy (negative damping). Even if the control actions are
bounded, the convergence is easily preserved.</p>
<p> This is a nonlinear controller that will push all system trajectories
to the unstable equilibrium. But does it make the unstable equilibrium
locally stable? With only this controller, the fixed point is <em>
attractive, but is not stable</em> -- just like our example above. Small
perturbations may cause the system to drive all of the way around the
circle in order to once again return to the unstable equilibrium. For
this reason, to actually balance the system, we'll have to switch to a
different controller once we get near the fixed point (an idea that we'll
discuss more in the next chapter).</p>
<example><h1>Energy Shaping for the Pendulum</h1>
<p>Take a minute to play around with the energy-shaping controller for
swinging up the pendulum</p>
<p><a target="pend"
href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/pend.ipynb#scrollTo=DZ2m35mEKUSr"><img
src="https://colab.research.google.com/assets/colab-badge.svg"
alt="Open In Colab"/></a>
</p>
<p>Make sure that you take a minute to look at the code which runs during
these examples. Note the somewhat arbitrary threshold for switching to
the balancing controller. We'll give a much more satisfying answer for
that in the <a href="lyapunov.html">chapter on Lyapunov methods</a>.</p>
</example>
<p>There are a few things that are extremely nice about this controller.
Not only is it simple, but it is actually incredibly robust to errors
we might have in our estimate of the model parameters, $m,$ $l,$ and $g.$
In fact, the only place that the model enters our control equation is in
the calculation of $\tilde{E} = \frac{1}{2} m l^2 \dot\theta^2 - mgl(1 +
\cos\theta)$, and the only property of this estimate that impacts
stability is the location of the orbit when $\tilde{E} = 0$, which is
$\frac{1}{2}l\dot\theta^2 = g(\cos\theta + 1).$ This doesn't depend at all
on our estimate of the mass, and only linearly on our estimates of the
length and gravity (and if one cannot measure the length of the pendulum
accurately, then a proper measuring device would make an excellent
investment). This is somewhat amazing; we will develop many
optimization-based algorithms capable of swinging up the pendulum, but it
will take a lot of work to find one that is as insensitive to the model
parameters.</p>
<p>If there is damping in the original system, of course we can cancel
this out, too, using $u = -k \dot\theta \tilde{E} + b\dot\theta.$ And
once again, the controller is quite robust if we have errors in the
estimated damping ratio.</p>
</subsection>
</section>
<section><h1>Exercises</h1>
<exercise><h1>Graphical Analysis</h1>
<figure>
<img width="60%" src="figures/exercises/graphical_analysis.svg"/>
<figcaption>First-order systems with multiple equilibria.</figcaption>
</figure>
Consider the first-order system $$\dot x = \begin{cases} - x^5 + 2 x^3 - x & \text{if} \quad x \leq 1, \\ 0 & \text{if} \quad 1 < x \leq 2, \\ - x + 2 & \text{if} \quad x > 2, \end{cases}$$ whose dynamics is represented in the figure above. Notice that, together with $x^*=-1$ and $x^*=0$, all the points in the closed interval $[1,2]$ are equilibria for this system. For each equilibrium point, determine whether it is unstable, stable i.s.L., asymptotically stable, or exponentially stable.
</exercise>
<exercise><h1>Basin of Attraction and Bifurcation Diagram</h1>
Consider the first-order system with polynomial dynamics $$\dot x = f(x) = - x^3 + 4x^2 +11x - 30.$$
<ol type="a">
<li> Sketch the graph of the function $f(x)$, and identify all the equilibrium points. (Feel free to plot this with a tool of your choice to check your work.)</li>
<li> For each equilibrium point, determine whether it is stable (i.s.L.) or unstable. For each one of the stable equilibria, identify the basin of attraction.</li>
<li> Consider an additive term $w$ in the system dynamics: $\dot x = f(x) + w$. As $w$ ranges from $0$ to $\infty$, determine how many stable and unstable equilibrium points the system has. Support your answer with a sketch of the bifurcation diagram for nonnegative values of $w$.</li>
</ol>
</exercise>
<exercise><h1>Enumerate Unstable Equilibria</h1>
We are given a dynamical system $\dot x = f(x)$ with a scalar state $x$. The dynamics $f(x)$ is unknown, but we are given two pieces of information:
<ul>
<li>the function $f(x)$ is continuous;</li>
<li>the system has exactly three stable (i.s.L.) equilibrium points.</li>
</ul>
Identify the minimum $n_{\text{min}}$ and the maximum $n_{\text{max}}$ number of unstable equilibria that the system can have. Support your answer with the sketch of two functions $f_{\text{min}}(x)$ and $f_{\text{max}}(x)$ that verify the requirements above and have, respectively, $n_{\text{min}}$ and $n_{\text{max}}$ unstable equilibria.
</exercise>
<exercise><h1>Attractivity vs Stability</h1>
<figure>
<img width="60%" src="figures/exercises/attractivity_vs_stability.svg"/>
<figcaption>Phase diagram of a two-dimensional system.</figcaption>
</figure>
This figure shows the phase portrait of the dynamical system \begin{align*} \dot x_1 &= x_1 (1-|\bx|) + x_2 \frac{x_1-|\bx|}{2|\bx|}, \\ \dot x_2 &= x_2 (1-|\bx|) - x_1 \frac{x_1-|\bx|}{2|\bx|}, \end{align*} where $|\bx| = \sqrt{x_1^2+x_2^2}$. To help you with the analysis of this system, we set up <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/pend/attractivity_vs_stability/attractivity_vs_stability.ipynb" target="_blank">this python notebook</a>. Take your time to understand the code in it, and then answer the following questions.
<ol type="a">
<li>Find all the equilibrium points of this system (no need to look outside the portion of state space depicted above). Use the notebook to double check your answer: simulate the evolution of the system setting the equilibrium points you identified as initial conditions.</li>
<li>Determine whether the equilibria you identified at the previous point are attractive and/or stable i.s.L. Explain briefly your reasoning, and feel free to include some plots generated with the notebook in your written answer.</li>
<li>This dynamical system is a (very) close relative of one of the systems we analyzed in this chapter. Can you guess which one is it? What is the connection between the two? Extra points: support your claim with a mathematical derivation.</li>
</ol>
</exercise>
<exercise><h1>Pendulum with Vibrating Base</h1>
Consider an actuated pendulum whose base (pivot of the rod) is forced to oscillate horizontally according to the harmonic law $h \sin(\omega t)$, where $h$ denotes the amplitude of the oscillation and $\omega$ the frequency. The equation of motion for this system is $$m l^2 \ddot \theta + m g l \sin \theta = m l \omega^2 h \sin (\omega t) \cos \theta + u.$$ (If this equation seems obscure to you, try to derive it using the <a href="multibody.html">Lagrangian approach</a>; but be careful, the kinetic energy $T(\theta, \dot \theta, t)$ of this system depends explicitly on time.) Our goal is to design a time-dependent control law $u = \pi (\theta, \dot \theta, t)$ that makes the pendulum spin at constant velocity $\dot \theta = 1$.
<ol type="a">
<li>Identify a desired closed-loop dynamics $\ddot \theta = f(\dot \theta)$, whose unique equilibrium point is stable and is located in $\dot \theta = 1$.</li>
<li>Use feedback cancellation to design a control law $u = \pi(\theta, \dot \theta, t)$ that makes the closed-loop dynamics coincide with the one chosen at the previous point.</li>
<li><a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/pend/vibrating_pendulum/vibrating_pendulum.ipynb" target="_blank">In this python notebook</a>, we set up a simulation environment to test your control law. Try to understand the structure of the code: this workflow is quite general, and it could be a good starting point for your future Drake projects. Implement your control law in the dedicated cell, and check your work using the animation and the plots at the end of the notebook.</li>
</ol>
</exercise>
<exercise id="hopfield"><h1>Hopfield Networks</h1>
Consider the following dynamical system
\begin{equation}
\dot{x} = A^T\,\text{softmax}(\beta\, A\, x) - x
\end{equation}
where $A\in\mathbb{R}^{m\times n}$ and $\beta\in\mathbb{R}$ are a constant matrix and a scalar, respectively, and the softmax function is given by $\text{softmax}(z)_i = \frac{e^{z_i}}{\sum_k e^{z_k}}$.
<ol type="a">
<li> Draw the phase portrait of the system with constants $\beta=5$ and $A=\begin{bmatrix}1&0 \\ 0&1 \\ -1&-1\end{bmatrix}$. </li>
<li> Identify (approximately by looking at the phase portrait) equilibriums of the above system. What are the stable and unstable equilibriums? </li>
<li> How are the stable equilibriums related to the matrix $A$? </li>
<li> Using <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/pend/hopfield_network/hopfield_network.ipynb" target="_blank">this python notebook</a>, implement an image recovery algorithm using Hopfield network. Modify the dynamical system above to memorize a subset of MNIST data set. Using a corrupted image as the initial condition, simulate the dynamical system to restore images. </li>
</ol>
</exercise>
<exercise id="learning_dynamics"><h1>Graphical Analysis of a Neural ODE</h1>
We've recently seen a trend in using neural networks to describe our dynamics function. The differential equation represented by the network can be very complex, and hard to analyze rigorously. However, in the special case of one-dimensional systems we can use graphical analysis to analyze our system.
For this exercise, you'll take a look at trying to learn the dynamics of the damped pendulum from data. This exercise has a coding component where you'll learn the basics of training a neural network in pytorch. There is then a written component where you'll perform a graphical analysis of the dynamics function represented by your trained network.
<ol type="a">
<li> In <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/pend/learning_dynamics/learning_dynamics.ipynb" target="_blank">this python notebook</a>, learn the dynamics of the damped pendulum from data.
<li> Within the range $\theta \in [-5.0, 5.0]$, which values of $(\theta, \dot \theta)$ represent fixed points? Which fixed points are stable and which are unstable? </li>
<li> What is one reason why our model might not fit the ground truth data exactly? </li>
<li> Does our model give us a reasonable approximation of the dynamics for all $(\theta, \dot \theta)$ pairs? Why or why not? </li>
</ol>
</exercise>
</section>
</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<div id="references"><section><h1>References</h1>
<ol>
<li id=Strogatz94>
<span class="author">Steven H. Strogatz</span>,
<span class="title">"Nonlinear Dynamics and Chaos: With Applications to Physics, Biology, Chemistry, and Engineering"</span>,Perseus Books
, <span class="year">1994</span>.
</li><br>
<li id=Seung00>
<span class="author">H. Sebastian Seung and Daniel D. Lee and Ben Y. Reis and David W. Tank</span>,
<span class="title">"The autapse: a simple illustration of short-term analog memory storage by tuned synaptic feedback"</span>,
<span class="publisher">Journal of Computational Neuroscience</span>, vol. 9, pp. 171-85, <span class="year">2000</span>.
</li><br>
</ol>
</section><p/>
</div>
<table style="width:100%;"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=intro.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=acrobot.html>Next Chapter</a></td>
</tr></table>
<div id="footer">
<hr>
<table style="width:100%;">
<tr><td><a href="https://accessibility.mit.edu/">Accessibility</a></td><td style="text-align:right">© Russ
Tedrake, 2021</td></tr>
</table>
</div>
</body>
</html>