forked from RussTedrake/underactuated
-
Notifications
You must be signed in to change notification settings - Fork 0
/
contact.html
211 lines (174 loc) · 11 KB
/
contact.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
<!DOCTYPE html>
<html>
<head>
<title>Underactuated Robotics</title>
<meta name="Underactuated Robotics" content="text/html; charset=utf-8;" />
<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('underactuated');">
<div data-type="titlepage">
<header>
<h1><a href="underactuated.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, 2020<br/>
<a href="tocite.html">How to cite these notes</a><br/>
</p>
</header>
</div>
<p><b>Note:</b> These are working notes used for <a
href="http://underactuated.csail.mit.edu/Spring2020/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2020 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=limit_cycles.html>Previous Chapter</a></td>
<td style="width:33%;text-align:center;"><a href=underactuated.html>Table of contents</a></td>
<td style="width:33%;text-align:right;"><a class="next_chapter" href=sysid.html>Next Chapter</a></td>
</tr></table>
<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 16"><h1>Planning and
Control through Contact</h1>
<p>So far we have developed a fairly strong toolbox for planning and control
with "smooth" systems -- systems where the equations of motion are described
by a function $\dot{\bx} = f(\bx,\bu)$ which is smooth everywhere. But our
<a href="simple_legs.html">discussion of the simple
models of legged robots</a> illustrated that the dynamics of making and
breaking contact with the world are more complex -- these are often modeled
as hybrid dynamics with impact discontinuities at the collision event and
constrained dynamics during contact (with either soft or hard
constraints).</p>
<p>My goal for this chapter is to extend our computational tools into this
richer class of models. Many of our core tools still work: trajectory
optimization, Lyapunov analysis (e.g. with sums-of-squares), and LQR all have
natural equivalents. </p>
<section><h1>Modeling and Simulating through Contact</h1>
<p>Recall how we modeled the dynamics of the simple legged robots. First,
we derived the equations of motion (independently) for each possible
contact configuration -- for example, in the spring-loaded inverted
pendulum (SLIP) model we had one set of equations governing the $(x,y)$
position of the mass during the flight phase, and a completely separate set
of equations written in polar coordinates, $(r,\theta)$, describing the
stance phase. Then we did a little additional work to describe the
transitions between these models -- e.g., in SLIP we transitioned from
flight to stance when the foot first touches the ground. When simulating
this model, it means that we have a discrete "event" which occurs at the
moment of foot collision, and an immediate discontinuous change to the
state of the robot (in this case we even change out the state variables).
</p>
<p>The language of <i>hybrid systems</i> gives us a rich language for
describing systems like this, and a suite of tools for simulating them.
<sidenote>The term "hybrid systems" means a lot of things to a lot of
people... here we use "hybrid" to mean both discrete and continuous, and
the particular systems we consider here are sometimes called
<i>autonomous</i> hybrid systems because the internal dynamics can cause
the discrete changes without any exogeneous input; in contrast to, for
instance, the model of a power-train where a change in gears comes as an
external input.</sidenote> More generally, we say that a hybrid systems is
described by a set of <i>modes</i> each described by (ideally smooth)
continuous dynamics, a set of <i>guards</i> which here are continuous
functions who's zero-level set describes the conditions which trigger an
event, and a set of <i>resets</i> which describe the discrete update to the
state that is triggered by the guard. Each guard is associated with a
particular mode, and we can have multiple guards per mode. Every guard has
at most one reset. You will occasionally here guards referred to as
"witness functions", since they play that role in simulation, and resets
are sometimes referred to as "transition functions".</p>
<p>The imagery that I like to keep in my head for hybrid systems is
illustrated below for a simple example of a robot's heel striking the
ground. A solution trajectory of the hybrid system has a continuous
trajectory inside each mode, punctionated by discrete updates when the
trajectory hits the zero-level set of the guard (here the distance between
the heel and the ground becomes zero), with the reset describing the
discrete change in the state variables.</p>
<figure> <img width="90%" src="figures/hybrid.svg"/>
<figcaption>Modeling contact as a hybrid system.</figcaption>
<todo>annotate with the mode, guard, reset language</todo> </figure>
<p>There is some work to do in order to derive the equations of motion in
this form. Do you remember how we did it for the <a
href="simple_legs.html">rimless wheel and compass
gait</a> examples? In both cases we assumed that exactly one foot was
attached to the ground and that it would not slip, this allowed us to write
the Lagrangian as if there was a pin joint attaching the foot to the ground
to obtain the equations of motion. For the SLIP model, we derived the
flight phase and stance phase using separate Lagrangian equations each with
different state representations. I would describe this as the <i>minimal
coordinates</i> modeling approach -- it is elegant and has some important
computational advantages that we will come to appreciate in the algorithms
below. But it's a lot of work! For instance, if we also wanted to consider
friction in the foot contact of the rimless wheel, we would have to derive
yet another set of equations to describe the sliding mode (adding, for
instance, a prismatic joint that moved the foot along the ramp), plus the
guards which compute the contact force for a given state and the distance
from the boundary of the friction cone, and on and on.</p>
<p>Fortunately, there is an alternative modeling approach for deriving the
modes, guards, and resets for contact that is less work. We can instead
model the robot in the <i>floating-base coordinates</i> -- we add a
fictitious six degree-of-freedom "floating-base" joint connecting some part
of the robot to the world (in planar models, we use just three
degrees-of-freedom, e.g. $(x,y,\theta)$). We can derive the equations of
motion for the floating-base robot once, without considering contact, then
add the additional constraints that come from being in contact as contact
forces which get applied to the bodies. The resulting manipulator equations
take the form \begin{equation}\bM({\bq})\ddot{\bq} +
\bC(\bq,\dot{\bq})\dot\bq = \btau_g(\bq) + \bB\bu + \sum_i \bJ_i^T(\bq)
\blambda_i,\end{equation} where $\blambda_i$ are the constraint forces and
$\bJ_i$ are the constraint Jacobians. Conveniently, if the guard function
in our contact equations is the signed distance from contact,
$\phi_i(\bq)$, then this Jacobian is simply $\bJ_i(\bq) =
\pd{\phi_i}{\bq}$. I've written the basic derivations for the common cases
(position constraints, velocity constraints, impact equations, etc) in the
<a href="multibody.html">appendix</a>. What is
important to understand here is that this is an alternative formulation for
the equations governing the modes, guards, and resets, but that is it no
longer a minimal coordinate system -- the equations of motion are written
in $2N$ state variables but the system might actually be constrained to
evolve only along a lower dimensional manifold (if we write the rimless
wheel equations with three configuration variables for the floating base,
it still only rotates around the toe when it is in stance and is inside the
friction cone). This will have implications for our algorithms.</p>
<p>Finally, there are many recipes for simulating these systems. At one
extreme, we have integrators that do explicit event detection -- we
register the guards of our system as "witness functions" for the event and
the integrator ensures that it obtains a high-accuracy integration step at
the event. For the complicated simulations that you see in computer
graphics, coarser approximations are often used, such as the time-stepping
approximations which solve a small optimization problem at every
integration step to compute time-averaged contact forces to summarize the
impact events without explicit event detection. I'll leave the details to
the <a href="multibody.html">appendix</a>, but will
use both approaches in this chapter for control and analysis. </p>
</section>
<section><h1>Trajectory Optimization</h1>
</section>
<section><h1>Randomized Motion Planning</h1></section>
<section><h1>Stabilizing a Fixed-Point</h1>
</section>
<section><h1>Stabilizing a Trajectory or Limit Cycle</h1>
<p>Transverse stabilization (build off limit cycle chapter)</p>
</section>
</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<table style="width:100%;"><tr style="width:100%">
<td style="width:33%;text-align:left;"><a class="previous_chapter" href=limit_cycles.html>Previous Chapter</a></td>
<td style="width:33%;text-align:center;"><a href=underactuated.html>Table of contents</a></td>
<td style="width:33%;text-align:right;"><a class="next_chapter" href=sysid.html>Next Chapter</a></td>
</tr></table>
<div id="footer">
<hr>
<table style="width:100%;">
<tr><td><em>Underactuated Robotics</em></td><td align="right">© Russ
Tedrake, 2020</td></tr>
</table>
</div>
</body>
</html>