13 Robotics Script

Introduction to Robotics
Marc Toussaint
February 4, 2014
This is a direct concatenation and reformatting of all lecture slides and exercises from
the Robotics course (winter term 2013/14, U Stuttgart), including a topic list to prepare
for exams. Sorry for the sometimes imperfect formatting.
Contents
1 Introduction 3
2 Kinematics 6
Kinematic map, Jacobian, inverse kinematics as optimization problem, motion profiles, trajectory interpolation, multiple
simultaneous tasks, special task variables, configuration/operational/null space, singularities
3 Path Planning 15
Path finding vs. trajectory optimization, local vs. global, Dijkstra, Probabilistic Roadmaps, Rapidly Exploring Random
Trees, non-holonomic systems, car system equation, path-finding for non-holonomic systems, control-based sampling,
Dubins curves
4 Path Optimization 26
very briefly
5 Dynamics 27
1D point mass, damping & oscillation, PID, dynamics of mechanical systems, Euler-Lagrange equation, Newton-Euler
recursion, general robot dynamics, joint space control, reference trajectory following, operational space control
6 Probability Basics 33
7 Mobile Robotics 36
State estimation, Bayes filter, odometry, particle filter, Kalman filter, SLAM, joint Bayes filter, EKF SLAM, particle SLAM,
graph-based SLAM
8 Control Theory 44
Topics in control theory, optimal control, HJB equation, infinite horizon case, Linear-Quadratic optimal control, Riccati
equations (differential, algebraic, discrete-time), controllability, stability, eigenvalue analysis, Lyapunov function
51section.9
10 Reinforcement Learning in Robotics 55
11 SKIPPED THIS TERM – Grasping (brief intro) 61

Force closure, alternative/bio-inspired views
12 SKIPPED THIS TERM – Legged Locomotion (brief intro) 64

Why legs, Raibert hopper, statically stable walking, zero moment point, human walking, compass gait, passive walker
1
2 Introduction to Robotics, Marc Toussaint—February 4, 2014
13 Exercises 69
14 Topic list 79
Introduction to Robotics, Marc Toussaint—February 4, 2014 3
1 Introduction
Why Robotics?
1:1
http://people.csail.mit.edu/nikolaus/drg/
1:5
Why Robotics?
• Commercial:
1:2 Industrial, health care, entertainment, agriculture, surgery, etc
• Critical view:
– International Committee for Robot Arms Control
http://www.icrac.co.uk/
– Noel Sharkey’s articles on robot ethics (Child care robots PePeRo..
http://www.nec.co.jp/products/robot/en/
1:6
http://www.saintpatrick.org/index.aspx/Health_Services/da_
Vinci_Surgery Robotics as intelligence research
1:3
AI in the real world
AI: Machine Learning, probabilistic reasoning, optimization

Real World: Interaction, manipulation, perception, navigation,
etc
1:7
(robot “wife” aico)

1:4
Why AI needs to go real world
Newton-Euler, joint space control, reference trajectory following, optimal operational space con-
trol
• Planning & optimization

goal: planning around obstacles, optimizing trajectories
Path finding vs. trajectory optimization, local vs. global, Dijkstra, Probabilistic Roadmaps, Rapidly
Exploring Random Trees, differential constraints, metrics; trajectory optimization, general cost
function, task variables, transition costs, gradient methods, 2nd order methods, Dynamic Pro-
Tunicates digest their brain once they settled! gramming
• Motion was the driving force to develop intelligence

– motion needs control & decision making ↔ fast information • Control Theory
processing
theory on designing optimal controllers
– motion needs anticipation & planning
– motion needs perception Topics in control theory, optimal control, HJB equation, infinite horizon case, Linear-Quadratic
– motion needs spatial representations
optimal control, Riccati equations (differential, algebraic, discrete-time), controllability, stability,
• Manipulation requires to acknowledge the structure (geometry, eigenvalue analysis, Lyapunov function
physics, objects) of the real world. Classical AI does not
1:8
• Mobile robots
Robotics as intelligence research goal: localize and map yourself
State estimation, Bayes filter, odometry, particle filter, Kalman filter, Bayes smoothing, SLAM,
• Machine Learning and AI are computational disciplines, which
joint Bayes filter, EKF SLAM, particle SLAM, graph-based SLAM
had great success with statistical modelling, analysis of data
sets, symbolic reasoning. But they have not solved autonomous
1:11
learning, acting & reasoning in real worlds.
• Neurosciences and psychology are descriptive sciences, either
on the biological or cognitive level, e.g. with geat sucesses to
describe and cure certain deceases. But they are not sufficient
to create intelligent systems. • Is this a practical or theoretical course?
• Robotics is the only synthetic discipline to understand intelli- “There is nothing more practical than a good theory.”
gent behavior in natural worlds. Robotic tells us what the actual (Vapnik, others...)
problems are when trying to organize behavior in natural worlds.
1:9 • Essentially, the whole course is about
History reducing real-world problems to mathematical problems
that can be solved efficiently

1:12
• little movie...
Books
( http://www.csail.mit.edu/videoarchive/history/aifilms http: There is no reference book for this lecture. But a basic well-
//www.ai.sri.com/shakey/ ) known standard text book is:
1:10
Four chapters
• Kinematics & Dynamics Craig, J.J.: Introduction to

robotics: mechanics and control.
goal: orchestrate joint movements for desired movement in task
Addison-Wesley New York, 1989.
spaces (3rd edition 2006)
Kinematic map, Jacobian, optimality principle of inverse kinematics, singularities, configura-

1:13
tion/operational/null space, multiple simultaneous tasks, special task variables, trajectory inter-
polation, motion profiles; 1D point mass, damping & oscillation, PID, general dynamic systems,
Books
An advanced text book on planning is this:
Steven M. LaValle: Planning Al-

gorithms. Cambridge University
Press, 2006.
online:
http://planning.cs.uiuc.edu/
1:14
Online resources
• VideoLecture by Oussama Khatib: http://academicearth.org/
courses/introduction-to-robotics http://www.virtualprofessors.
com/introduction-to-robotics-stanford-cs223a-khatib
(focus on kinematics, dynamics, control)
• Oliver Brock’s lecture http://courses.robotics.tu-berlin.de/
mediawiki/index.php/Robotics:_Schedule_WT09
• Stefan Schaal’s lecture Introduction to Robotics: http://www-clmc.
usc.edu/Teaching/TeachingIntroductionToRoboticsSyllabus
(focus on control, useful: Basic Linear Control Theory (analytic solution
to simple dynamic model → PID), chapter on dynamics)
• Chris Atkeson’s “Kinematics, Dynamic Systems, and Control” http:
//www.cs.cmu.edu/˜cga/kdc/
(uses Schaal’s slides and LaValle’s book, useful: slides on 3d kinemat-
ics http://www.cs.cmu.edu/˜cga/kdc/ewhitman1.pptx)
• CMU lecture “introduction to robotics” http://www.cs.cmu.edu/afs/
cs.cmu.edu/academic/class/16311/www/current/syllabus.
html
(useful: PID control, simple BUGs algorithms for motion planning, non-
holonomic constraints)
• Handbook of Robotics (partially online at Google books) http://tiny.

cc/u6tzl
• LaValle’s Planning Algorithms http://planning.cs.uiuc.edu/
1:15
Organization
• Course webpage:
http://ipvs.informatik.uni-stuttgart.de/mlr/marc/teaching/
– Slides, exercises & software (C++)
– Links to books and other resources
• Secretary, admin issues:

Carola Stahl, Carola.Stahl@ipvs.uni-stuttgart.de, room 2.217
• Rules for the tutorials:

– Doing the exercises is crucial!
– At the beginning of each tutorial:
– sign into a list
– mark which exercises you have (successfully) worked on
– Students are randomly selected to present their solutions
– You need 50% of completed exercises to be allowed to the
exam
1:16
2 Kinematics Pose (position & orientation)
Kinematic map, Jacobian, inverse kinematics as optimization prob-

lem, motion profiles, trajectory interpolation, multiple simultaneous
tasks, special task variables, configuration/operational/null space, sin-
gularities
• Two “types of robotics”:

• A pose is described by a translation p ∈ R3 and a rotation R ∈
1) Mobile robotics – is all about localization & mapping SO(3)
2) Manipulation – is all about interacting with the world – R is an orthonormal matrix (orthogonal vectors stay orthog-
onal, unit vectors stay unit)
[0) Kinematic/Dynamic Motion Control: same as 2) without ever making
– R-1 = R⊤
it to interaction..]
– columns and rows are orthogonal unit vectors
– det(R) = 1
 
• Typical manipulation robots (and animals) are kinematic trees R
 11
R12 R13 
– R=  R21

R 22 R23 


Their pose/state is described by all joint angles R31 R32 R33
2:5
2:1
Frame and coordinate transforms

Basic motion generation problem
• Move all joints in a coordinated way so that the endeffector

makes a desired movement
• Let (o, e1:3 ) be the world frame, (o′ , e′1:3 ) be the body’s frame.
The new basis vectors are the columns in R, that is, e′1 =
R11 e1 + R21 e2 + R31 e3 , etc,
01-kinematics: ./x.exe -mode 2/3/4
• x = coordinates in world frame (o, e1:3 )
2:2
x′ = coordinates in body frame (o′ , e′1:3 )
p = coordinates of o′ in world frame (o, e1:3 )
Outline
x = p + Rx′
• Basic 3D geometry and notation 2:6
Rotations
• Kinematics: φ : q 7→ y
• Inverse Kinematics: y ∗ 7→ q ∗ = argminq ||φ(q)−y ∗ ||2C +||q−q0 ||2W • Rotations can alternatively be represented as
• Basic motion heuristics: Motion profiles – Euler angles – NEVER DO THIS!
– Rotation vector
– Quaternion – default in code
• Additional things to know
• See the “geometry notes” for formulas to convert, concatenate
– Many simultaneous task variables
& apply to vectors
– Singularities, null space,
2:7
2:3
Homogeneous transformations
Basic 3D geometry & notation
• xA = coordinates of a point in frame A
2:4
xB = coordinates of a point in frame B
• Translation and rotation: xA = t + RxB Kinematics
• Homogeneous transform T ∈ R4×4 :

eff
B
 
TAB = R t joint B'

  transf.
0 1 A
A'
C'
relative
      C eff.
B B
xA = TAB xB =  R t  x 
= Rx + t  offset
     
0 1 1 1
link
transf.
W
in homogeneous coordinates, we append a 1 to all coordinate
vectors
2:8
• A kinematic structure is a graph (usually tree or chain)

Is TAB forward or backward? of rigid links and joints
• TAB describes the translation and rotation of frame B relative

TW eff (q) = TW A TAA′ (q) TA′ B TBB ′ (q) TB ′ C TCC ′ (q) TC ′
to A
That is, it describes the forward FRAME transformation (from A
2:12
to B)
Joint types
• TAB describes the coordinate transformation from xB to xA
That is, it describes the backward COORDINATE transformation • Joint transformations: TAA′ (q) depends on q ∈ Rn
• Confused? Vectors (and frames) transform covariant, coordi- revolute joint: joint angle q ∈ R determines rotation about x-
nates contra-variant. See “geometry notes” or Wikipedia for axis:
more details, if you like. 1


0 0

0


2:9 TAA′ (q) = 0

 cos(q) − sin(q) 0

 
0 


sin(q) cos(q) 0

0 0 0 1
Composition of transforms
prismatic joint: offset q ∈ R determines translation along x-axis:
 
1 
0 0 q

TAA′ (q) = 0

 1 0 0

 
0 


0 1 0

0 0 0 1
others: screw (1dof), cylindrical (2dof), spherical (3dof), univer-

sal (2dof)
2:13
TW C = TW A TAB TBC
xW = TW A TAB TBC xC
2:10
Kinematics
2:11 2:14
Kinematic Map • The i-th joint is

 
located at pi = tW i (q) and has rotation axis
1
 
ai = RW i (q)0
 

 
0
• For any joint angle vector q ∈ Rn we can compute TW eff (q) • We consider an infinitesimal variation δqi ∈ R of the ith joint and
by forward chaining of transformations see how an endeffector position peff = φpos
eff,v (q) and attached
vector aeff = φvec

eff,v (q) change.
TW eff (q) gives us the pose of the endeffector in the world frame 2:18
Jacobian for a rotational joint

• The two most important examples for a kinematic map φ are Consider a variation δqi
eff
→ the whole sub-tree ro-

axis
point
tates
1) A point v on the endeffector transformed to world coordinates: i-th joint
δpeff = [ai × (peff − pi )] δqi

φpos
eff,v (q) = TW eff (q) v ∈ R3 δaeff = [ai × aeff ] δqi
2) A direction v ∈ R3 attached to the endeffector transformed to ⇒ PositionJacobian: 

world: ⇒ Vector Jacobian:
[a1 × (peff − p1 )]
[a2 × (peff − p2 )]
[an × (peff − pn )]
 
   
 
3  
φvec
eff,v (q) = RW eff (q) v ∈R    
[a1 × aeff ]
[a2 × aeff ]
[an × aeff ]
   
..
.
   
   
pos
Jeff,v (q) = 




 ∈ R3×n Jeff,v
vec
(q) = 





∈
..
.
 
Where RAB is the rotation in TAB . 









 
 
 
2:15 



2:19
Kinematic Map
Jacobian
• In general, a kinematic map is any (differentiable) mapping
• To compute the Jacobian of some endeffector position or vector,

φ : q 7→ y
we only need to know the position and rotation axis of each joint.
that maps to some arbitrary feature y ∈ Rd of the pose q ∈ Rn
2:16 • The two kinematic maps φpos and φvec are the most important
two examples – more complex geometric features can be com-
Jacobian
puted from these, as we will see later.
2:20
• When we change the joint angles, δq, how does the effector
position change, δy? Inverse Kinematics
2:21
• Given the kinematic map y = φ(q) and its Jacobian J(q) =
∂
∂q
φ(q), we have: Inverse Kinematics problem
δy = J(q) δq
• Generally, the aim is to find a robot configuration q such that
φ(q) = y ∗

∂φ1 (q) ∂φ1 (q) ∂φ1 (q) 
 ∂q1 ∂q2
... ∂qn  • Iff φ is invertible
 
 ∂φ2 (q) ∂φ2 (q) ∂φ2 (q) 
∂ 
 ∂q ... ∂qn 

J(q) = φ(q) =   1 ∂q2 

 ∈R d×n q ∗ = φ-1 (y ∗ )
∂q 
 .. .. 

 


. . 

 ∂φ (q) ∂φd (q) ∂φd (q) 
d
∂q1 ∂q2
... ∂qn
• But in general, φ will not be invertible:
2:17
Jacobian for a rotational joint 1) The pre-image φ-1 (y ∗ ) = may be empty: No configuration
can generate the desired y ∗
eff
axis
point
2) The pre-image φ-1 (y ∗ ) may be large: many configurations
i-th joint
can generate the desired y ∗

2:22
Inverse Kinematics as optimization problem • We can derive the optimum as
f (q) = ||φ(q) − y ∗ ||2C + ||q − q0 ||2W

• We formalize the inverse kinematics problem as an optimization
= ||y0 − y ∗ + J (q − q0 )||2C + ||q − q0 ||2W
problem
∂
f (q) = 0⊤ = 2(y0 − y ∗ + J (q − q0 ))⊤CJ + 2(q − q0 )T W
∂q
q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W J⊤C (y ∗ − y0 ) = (J⊤CJ + W ) (q − q0 )
q
q ∗ = q0 + J ♯ (y ∗ − y0 )
∗
• The 1st term ensures that we find a configuration even if y is
with J ♯ = (J⊤CJ + W )-1 J⊤C = W -1 J⊤(JW -1 J⊤ + C -1 )-1 (Woodbury
not exactly reachable
identity)
The 2nd term disambiguates the configurations if there are many
φ-1 (y ∗ )
– For C → ∞ and W = I, J ♯ = J⊤(JJ⊤)-1 is called pseudo-
inverse
– W generalizes the metric in q-space
– C regularizes this pseudo-inverse (see later section on sin-
gularities)
2:26
“Small step” application
• This approximate solution to IK makes sense

2:23
– if the local linearization of φ at q0 is “good”
– if q0 and q ∗ are close
Inverse Kinematics as optimization problem • This equation is therefore typically used to iteratively compute
small steps in configuration space
q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W qt+1 = qt + J ♯ (yt+1

∗
− φ(qt ))
q
∗
where the target yt+1 moves smoothly with t
• The formulation of IK as an optimization problem is very power- 2:27
ful and has many nice properties

Example: Iterating IK to follow a trajectory
• We will be able to take the limit C → ∞, enforcing exact φ(q) =
y ∗ if possible • Assume initial posture q0 . We want to reach a desired endeff
• Non-zero C -1
and W corresponds to a regularization that en- position y ∗ in T steps:
sures numeric stability
Input: initial state q0 , desired y ∗ , methods φpos and J pos
• Classical concepts can be derived as special cases: Output: trajectory q0:T
– Null-space motion 1: Set y0 = φpos (q0 ) // starting endeff position
2: for t = 1 : T do
– regularization; singularity robutness
3: y ← φpos (qt-1 ) // current endeff position
– multiple tasks 4: J ← J pos (qt-1 ) // current endeff Jacobian
– hierarchical tasks 5: ŷ ← y0 + (t/T )(y ∗ − y0 ) // interpolated endeff target
6: qt = qt-1 + J ♯ (ŷ − y) // new joint positions
2:24
7: Command qt to all robot motors and compute all
TW i (qt )
Solving Inverse Kinematics 8: end for
01-kinematics: ./x.exe -mode 2/3

• The obvious choice of optimization method for this problem is
Gauss-Newton, using the Jacobian of φ
• We first describe just one step of this, which leads to the classi- • Why does this not follow the interpolated trajectory ŷ0:T exactly?
cal equations for inverse kinematics using the local Jacobian... – What happens if T = 1 and y ∗ is far?
2:25 2:28
Solution using the local linearization Two additional notes

• What if we linearize at some arbitrary q ′ instead of q0 ?
• When using the local linearization of φ at q0 ,
φ(q) ≈ y ′ + J (q − q ′ ) , y ′ = φ(q ′ )
∗
φ(q) ≈ y0 + J (q − q0 ) , y0 = φ(q0 ) q = argmin ||φ(q) − y ∗ ||2C + ||q − q ′ + (q ′ − q0 )||2W
q
= q ′ + J ♯ (y ∗ − y ′ ) + (I − J ♯ J) h , h = q0 − q ′ (1) Motion profiles
Note that h corresponds to the classical concept of null space • Generally, let’s define a motion profile as a mapping
motion
MP : [0, 1] 7→ [0, 1]
• What if we want to find the exact (local) optimum? E.g. what if with MP(0) = 0 and MP(1) = 1 such that the interpolation is
∗
we want to compute a big step (where q will be remote from q) given as
and we cannot not rely only on the local linearization approxi- xt = x0 + MP(t/T ) (xT − x0 )
mation?
– Iterate equation (1) (optionally with a step size < 1 to en-
sure convergence) by setting the point y ′ of linearization to • For example
the current q ∗
– This is equivalent to the Gauss-Newton algorithm MPramp (s) = s
2:29 1
MPsin (s) = [1 − cos(πs)]
2
Where are we? 2:33
• We’ve derived a basic motion generation principle in robotics Joint space interpolation
from
– an understanding of robot geometry & kinematics
1) Optimize a desired final configuration qT :
– a basic notion of optimality
Given a desired final task value yT , optimize a final joint state qT to
minimize the function
• In the remainder: f (qT ) = ||qT − q0 ||2W/T + ||yT − φ(qT )||2C

A. Heuristic motion profiles for simple trajectory generation
B. Extension to multiple task variables – The metric T1 W is consistent with T cost terms with step metric W .
C. Discussion of classical concepts – In this optimization, qT will end up remote from q0 . So we need to
iterate Gauss-Newton, as described on slide 2.
– Singularity and singularity-robustness
– Nullspace, task/operational space, joint space
– “inverse kinematics” ↔ “motion rate control” 2) Compute q0:T as interpolation between q0 and qT :
2:30 Given the initial configuration q0 and the final qT , interpolate on a straight
line with a some motion profile. E.g.,
Heuristic motion profiles qt = q0 + MP(t/T ) (qT − q0 )
2:31 2:34
Heuristic motion profiles Task space interpolation
• Assume initially x = 0, ẋ = 0. After 1 second you want x =

1, ẋ = 0. 1) Compute y0:T as interpolation between y0 and yT :
Given a initial task value y0 and a desired final task value yT , interpolate
How do you move from x = 0 to x = 1 in one second?
on a straight line with a some motion profile. E.g,
yt = y0 + MP(t/T ) (yT − y0 )
2) Project y0:T to q0:T using inverse kinematics:

Given the task trajectory y0:T , compute a corresponding joint trajectory
q0:T using inverse kinematics
qt+1 = qt + J ♯ (yt+1 − φ(qt ))
(As steps are small, we should be ok with just using this local lineariza-
tion.)
The sine profile xt = x0 + 21 [1 − cos(πt/T )](xT − x0 ) is a com-
2:35
promise for low max-acceleration and max-velocity
Taken from http://www.20sim.com/webhelp/toolboxes/mechatronics_
toolbox/motion_profile_wizard/motionprofiles.htm peg-in-a-hole demo

2:32 2:36
√  
Multiple tasks ̺ (φ (q) − y1∗ ) 
√ 1 1 
 ∗  P
where Φ(q) := ̺2 (φ2 (q) − y2 ) 

  ∈R i di
 
.. 



2:37 .
2:40
Multiple tasks
Multiple tasks
• We can “pack” together all tasks in one “big task” Φ.
Example: We want to control the 3D position of the left hand and of the
right hand. Both are “packed” to one 6-dimensional task vector which
becomes zero if both tasks are fulfilled.
• The big Φ is scaled/normalized in a way that

– the desired value is always zero
– the cost metric is I
∂Φ(q0 )
2:38
• Using the local linearization of Φ at q0 , J = ∂q
, the optimum
is
Multiple tasks q ∗ = argmin ||q − q0 ||2W + ||Φ(q)||2

q
≈ q0 − (J⊤J + W )-1 J⊤ Φ(q0 ) = q0 − J # Φ(q0 )
2:41
Multiple tasks
• We learnt how to “puppeteer a

robot”
RightHand • We can handle many task vari-
position ables (but specifying their preci-
LeftHand sions ̺i becomes cumbersome...)
position
RightHand • In the remainder:
position
LeftHand A. Classical limit of “hierarchical IK”
position
and nullspace motion
B. What are interesting task vari-
2:39 ables?
2:42
Multiple tasks Hierarchical IK & nullspace motion
• Assume we have m simultaneous tasks; for each task i we • In the classical view, tasks should be executed exactly, which means
taking the limit ̺i → ∞ in some prespecified hierarchical order.
have: • We can rewrite the solution in a way that allows for such a hierarchical
– a kinematic mapping yi = φi (q) ∈ Rdi limit:
– a current value yi,t = φi (qt ) • One task plus “nullspace motion”:
– a desired value yi∗ f (q) = ||q − a||2W + ̺1 ||J1 q − y1 ||2

– a precision ̺i (implying a task cost metric Ci = ̺i I) ∝ ||q − â||2c
W
c = W + ̺1 J⊤J1 ,
W c -1 (W a + ̺1 J⊤y1 ) = J # y1 + (I − J # J1 )a
â = W
1 1 1 1
• Each task contributes a term to the objective function J1# = (W/̺1 + J⊤ -1 ⊤

1 J 1 ) J1
• Two tasks plus nullspace motion:

q ∗ = argmin ||q − q0 ||2W + ̺1 ||φ1 (q) − y1∗ ||2 + ̺2 ||φ2 (q) − y2∗ ||2 + · · ·
q
f (q) = ||q − a||2W + ̺1 ||J1 q − y1 ||2 + ̺2 ||J2 q − y2 ||2
which we can also write as = ||q − â||2c + ||J1 q + Φ1 ||2

W
q ∗ = J1# y1 + (I − J1# J1 )[J2# y2 + (I − J2# J2 )a]

q ∗ = argmin ||q − q0 ||2W + ||Φ(q)||2 J2# = (W/̺2 + J⊤ -1 ⊤
2 J2 ) J2 , J1# = (W
c /̺1 + J⊤J1 )-1 J⊤
1 1
q
• etc... – the short notation “A × p” means that each column in A

2:43
takes the cross-product with p.
2:47
Hierarchical IK & nullspace motion

Relative position
• The previous slide did nothing but rewrite the nice solution q ∗ = Position of a point on link i relative to point on link j
# dimension d = 3
−J Φ(q0 ) (for the “big” Φ) in a strange hierarchical way that
allows to “see” nullspace projection parameters link indices i, j, point offset v in i and w in j
kin. map φpos pos
iv|jw (q) = Rj (φiv − φjw )
-1 pos
pos pos pos

Jacobian Jiv|jw (q) = Rj-1 [Jiv − Jjw − Aj × (φpos pos
iv − φjw )]
• The benefit of this hierarchical way to write the solution is that
one can take the hierarchical limit ̺i → ∞ and retrieve classical
Derivation:
hierarchical IK For y = Rp the derivative w.r.t. a rotation around axis a is y ′ = Rp′ + R′ p =
Rp′ + a × Rp. For y = R-1 p the derivative is y ′ = R-1 p′ − R-1 (R′ )R-1 p =
R-1 (p′ −a×p). (For details see http://ipvs.informatik.uni-stuttgart.
de/mlr/marc/notes/3d-geometry.pdf)
• The drawbacks are:
– It is somewhat ugly 2:48
– In practise, I would recommend regularization in any case
(for numeric stability). Regularization corresponds to NOT
taking the full limit ̺i → ∞. Then the hierarchical way to
Relative vector
write the solution is unnecessary. (However, it points to
Vector attached to link i relative to link j
a “hierarchical regularization”, which might be numerically
dimension d=3
more robust for very small regularization?)
parameters link indices i, j, attached vector v in i
– The general solution allows for arbitrary blending of tasks
kin. map φvec -1 vec
iv|j (q) = Rj φiv
vec vec
2:44 Jacobian -1
Jiv|j (q) = Rj [Jiv − Aj × φvec
iv ]
2:49
What are interesting task variables?

Alignment
Alignment of a vector attached to link i with a reference v ∗
The following slides will define 10 different types of task vari- dimension d=1
ables. parameters link index i, attached vector v, world refer-
ence v ∗
This is meant as a reference and to give an idea of possibilities... φalign ∗⊤ vec
kin. map iv (q) = v φiv
align
2:45 Jacobian Jiv (q) = v ∗⊤ Jivvec
Position
Note: φalign = 1 ↔ align φalign = −1 ↔ anti-align φalign = 0 ↔ orthog.
Position of some point attached to link i 2:50

dimension d=3
parameters link index i, point offset v Relative Alignment
kin. map φpos
iv (q) = TW i v
pos
Jacobian Jiv (q)·k = [k ≺ i] ak × (φpos
iv (q) − pk )
Alignment a vector attached to link i with vector attached to j
dimension d=1
parameters link indices i, j, attached vectors v, w
Notation: kin. map φalign vec ⊤ vec
iv|jw (q) = (φjw ) φiv
align ⊤ vec vec⊤ vec
– ak , pk are axis and position of joint k Jacobian Jiv|jw (q) = (φvec
jw ) Jiv + φiv Jjw
– [k ≺ i] indicates whether joint k is between root and link i 2:51
– J·k is the kth column of J

2:46
Joint limits
Penetration of joint limits
Vector dimension d=1
parameters joint limits qlow , qhi , margin m
Vector attached to link i 1
Pn + +
kin. map φlimits (q) = m i=1 [m−qi +qlow ] +[m+qi −qhi ]
dimension d=3 Jacobian 1 1
Jlimits (q)1,i = − m [m − qi + qlow > 0] + m [m +
parameters link index i, attached vector v qi − qhi > 0]
kin. map φvec
iv (q) = RW i v
vec
Jacobian Jiv (q) = Ai × φvec
iv (q)
[x]+ = x > 0?x : 0 [· · · ]: indicator function
Notation:
2:52
– Ai is a matrix with columns (Ai )·k = [k ≺ i] ak containing
the joint axes or zeros
Collision limits – “inverse kinematics” ↔ “motion rate control”

2:57
Penetration of collision limits
dimension d=1
parameters margin m Singularity
1
PK a b +
kin. map φcol (q) = m
P k=1 [m − |pk − pk |]
1 K a b
Jacobian Jcol (q) = m k=1 [m − |pk − pk | > 0] • In general: A matrix J singular ⇐⇒ rank(J) < d
pa −pb – rows of J are linearly dependent
(−Jppos pos ⊤ k
a + J b )
p
k
|pa −pb |
k k k k
– dimension of image is < d
– δy = Jδq ⇒ dimensions of δy limited
A collision detection engine returns a set {(a, b, pa , pb )K
k=1 } of potential
– Intuition: arm fully stretched
collisions between link ak and bk , with nearest points pa b
k on a and pk on
b.
2:53 • Implications:
det(JJ⊤) = 0
Center of gravity → pseudo-inverse J⊤(JJ⊤)-1 is ill-defined!
Center of gravity of the whole kinematic structure → inverse kinematics δq = J⊤(JJ⊤)-1 δy computes “infinite”
dimension d=3
steps!
parameters (none)
P
kin. map φcog (q) = i massi φposici
P pos
Jacobian J cog (q) = i massi Jic i • Singularity robust pseudo inverse J⊤(JJ⊤ + ǫI)-1
The term ǫI is called regularization
ci denotes the center-of-mass of link i (in its own frame) • Recall our general solution (for W = I)
2:54 J ♯ = J⊤(JJ⊤ + C -1 )-1
is already singularity robust
Homing
2:58
The joint angles themselves
dimension d=n Null/task/operational/joint/configuration spaces
parameters (none)
kin. map φqitself (q) = q
• The space of all q ∈ Rn is called joint/configuration space
Jacobian Jqitself (q) = In
The space of all y ∈ Rd is called task/operational space
Usually d < n, which is called redundancy
Example: Set the target y ∗ = 0 and the precision ̺ very low → this task
describes posture comfortness in terms of deviation from the joints’ zero
position. In the classical view, it induces “nullspace motion”.
• For a desired endeffector state y ∗ there exists a whole manifold
2:55
(assuming φ is smooth) of joint configurations q:
Task variables – conclusions

nullspace(y ∗ ) = {q | φ(q) = y ∗ }
• There is much space for creativity in

defining task variables! Many are ex-
tensions of φpos and φvec and the Ja- • We found earlier that
cobians combine the basic Jacobians.
• What the right task variables are to

q ∗ = argmin ||q − a||2W + ̺||Jq − y ∗ ||2
q
design/describe motion is a very hard
problem! In what task space do hu- = J # y ∗ + (I − J # J)a , J # = (W/̺ + J⊤J)-1 J⊤
mans control their motion? Possible to
learn from data (“task space retrieval”)
or perhaps via Reinforcement Learn-
In the limit ̺ → ∞ it is guaranteed that Jq = y ∗ (we are exacty
nearest
distance
LeftHand ing. on the manifold). The term a introduces additional “nullspace
position
• In practice: Robot motion design (in- motion”.

cluding grasping) may require cumber- 2:59
some hand-tuning of such task vari-
ables.
2:56
Inverse Kinematics and Motion Rate Control
Discussion of classical concepts Some clarification of concepts:
– Singularity and singularity-robustness • The notion “kinematics” describes the mapping φ : q 7→ y,

– Nullspace, task/operational space, joint space which usually is a many-to-one function.
• The notion “inverse kinematics” in the strict sense describes

some mapping g : y 7→ q such that φ(g(y)) = y, which usu-
ally is non-unique or ill-defined.
• In practice, one often refers to δq = J ♯ δy as inverse kinemat-
ics.
• When iterating δq = J ♯ δy in a control cycle with time step τ

(typically τ ≈ 1 − 10 msec), then ẏ = δy/τ and q̇ = δq/τ and
q̇ = J ♯ ẏ. Therefore the control cycle effectively controls the
endeffector velocity—this is why it is called motion rate control.
2:60
3 Path Planning Path finding examples
Path finding vs. trajectory optimization, local vs. global, Dijkstra, Prob-
abilistic Roadmaps, Rapidly Exploring Random Trees, non-holonomic
systems, car system equation, path-finding for non-holonomic sys-
tems, control-based sampling, Dubins curves
Path finding examples
S. M. LaValle and J. J. Kuffner. Randomized Kinodynamic Plan-

ning. International Journal of Robotics Research, 20(5):378–
Alpha-Puzzle, solved with James Kuffner’s RRTs 400, May 2001.
3:1 3:4
Path finding examples

Feedback control, path finding, trajectory op-
tim.
path finding
trajectory optimization
feedback control
start goal
• Feedback Control: E.g., qt+1 = qt + J ♯ (y ∗ − φ(qt ))

• Trajectory Optimization: argminq0:T f (q0:T )
J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue.
• Path Finding: Find some q0:T with only valid configurations
Footstep Planning Among Obstacles for Biped Robots. Proc.
3:5
IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS),
2001.
3:2 Control, path finding, trajectory optimization
Path finding examples • Combining methods:
1) Path Finding
2) Trajectory Optimization (“smoothing”)
3) Feedback Control
• Many problems can be solved with only feedback control (though

not optimally)
T. Bretl. Motion Planning of Multi-Limbed Robots Subject to • Many more problems can be solved locally optimal with only
Equilibrium Constraints: The Free-Climbing Robot Problem. In- trajectory optimization
ternational Journal of Robotics Research, 25(4):317-342, Apr • Tricky problems need path finding: global search for valid paths
2006. 3:6
3:3
Outline
• Heuristics & Discretization (slides from Howie CHoset’s CMU A better bug?
lectures)
– Bugs algorithm “Bug 2” Algorithm
– Potentials to guide feedback control 1) head toward goal on the m-line

2) if an obstacle is in the way,
– Dijkstra Start
follow it until you encounter the
m-line again.
3) Leave the obstacle and continue
• Sample-based Path Finding toward the goal
– Probabilistic Roadmaps
– Rapidly Exploring Random Trees
3:7 Goal
NO! How do we fix this?
3:10
A better bug?
m-line
“Bug 2” Algorithm A better bug?
1) head toward goal on the m-line
2) if an obstacle is in the way, “Bug 2” Algorithm
m-line again. 1) head toward goal on the m-line
3) Leave the obstacle and continue 2) if an obstacle is in the way,
Start
toward the goal follow it until you encounter the
m-line again closer to the goal.
3) Leave the obstacle and continue
toward the goal
OK ?
Goal Better or worse than Bug1?

3:8
3:11
BUG algorithms – conclusions

A better bug?
• Other variants: TangentBug, VisBug, RoverBug, WedgeBug,
“Bug 2” Algorithm
...
1) head toward goal on the m-line
• only 2D! (TangentBug has extension to 3D)
2) if an obstacle is in the way,
Start
• Guaranteed convergence
m-line again.
3) Leave the obstacle and continue • Still active research:
toward the goal
K. Taylor and S.M. LaValle: I-Bug: An Intensity-Based Bug
Algorithm
Goal Better or worse than Bug1?
⇒ Useful for minimalistic, robust 2D goal reaching

– not useful for finding paths in joint space
3:9
3:12
Start-Goal Algorithm: Local Minimum Problem with the Charge Analogy

Potential Functions
3:13
3:16
Repulsive Potential Potential fields – conclusions
• Very simple, therefore popular

• In our framework: Combining a goal (endeffector) task variable,
with a constraint (collision avoidance) task variable; then using
inv. kinematics is exactly the same as “Potential Fields”
⇒ Does not solve locality problem of feedback control.
3:17
3:14
The Wavefront in Action (Part 2)

Total Potential Function
• Now repeat with the modified cells
U (q ) = U att (q ) + U rep (q )
– This will be repeated until no 0’s are adjacent to cells
with values >= 2
• 0’s will only remain when regions are unreachable
F (q ) = −∇U (q )
+ =
3:18
3:15

• Starting with the goal, set all adjacent cells with
“0” to the current cell + 1 • And again...
– 4-Point Connectivity or 8-Point Connectivity?
– Your Choice. We’ll use 8-Point Connectivity in our example
3:19
3:22

• Now repeat with the modified cells
– This will be repeated until no 0’s are adjacent to cells • And again until...
with values >= 2
• 0’s will only remain when regions are unreachable
3:20
3:23
The Wavefront in Action (Part 3) The Wavefront in Action (Done)

• Repeat again... • You’re done
– Remember, 0’s should only remain if unreachable
regions exist
3:21 3:24
Probabilistic Road Maps
The Wavefront, Now What?

• To find the shortest path, according to your metric, simply always
move toward a cell with a lower number
– The numbers generated by the Wavefront planner are roughly proportional to their
distance from the goal
Two
possible
shortest
paths
shown
[Kavraki, Svetska, Latombe,Overmars, 95]
Probabilistic Road Map
– generates a graph G = (V, E) of configurations
3:25 – such that configurations along each edges are ∈ Qfree
3:29
Dijkstra Algorithm
• Is efficient in discrete domains Probabilistic Road Maps

– Given start and goal node in an arbitrary graph
– Incrementally label nodes with their distance-from-start
• Produces optimal (shortest) paths
• Applying this to continuous domains requires discretization

– Grid-like discretization in high-dimensions is daunting! (curse
of dimensionality)
– What are other ways to “discretize” space more efficiently?
3:26
Sample-based Path Finding Given the graph, use (e.g.) Dijkstra to find path from qstart to
qgoal .
3:27
3:30
Probabilistic Road Maps
Probabilistic Road Maps – generation
Input: number n of samples, number k number of nearest

neighbors
Output: PRM G = (V, E)
1: initialize V = ∅, E = ∅
2: while |V | < n do // find n collision free points qi
3: q ← random sample from Q
4: if q ∈ Qfree then V ← V ∪ {q}
5: end while
6: for all q ∈ V do // check if near points can be connected
7: Nq ← k nearest neighbors of q in V
8: for all q ′ ∈ Nq do
[Kavraki, Svetska, Latombe,Overmars, 95] 9: if path(q, q ′ ) ∈ Qfree then E ← E ∪ {(q, q ′ )}
10: end for
11: end for
q ∈ Rn describes configuration
Qfree is the set of configurations without collision where path(q, q ′ ) is a local planner (easiest: straight line)
3:31
3:28
Local Planner
illustration from O. Brock’s lecture
Gaussian: q1 ∼ U; q2 ∼ N(q1 , σ); if q1 ∈ Qfree and q2 6∈ Qfree , add q1

tests collisions up to a specified resolution δ (or vice versa).
3:32 Bridge: q1 ∼ U; q2 ∼ N(q1 , σ); q3 = (q1 + q2 )/2; if q1 , q2 6∈ Qfree and
q3 ∈ Qfree , add q3 .
Problem: Narrow Passages • Sampling strategy can be made more intelligence: “utility-based
sampling”
• Connection sampling
(once earlier sampling has produced connected components)
3:35
Probabilistic Roadmaps – conclusions
• Pros:
– Algorithmically very simple
– Highly explorative
The smaller the gap (clearance ̺) the more unlikely to sample – Allows probabilistic performance guarantees
such points. – Good to answer many queries in an unchanged environment

3:33
• Cons:
PRM theory – Precomputation of exhaustive roadmap takes a long time
(but not necessary for “Lazy PRMs”)
(for uniform sampling in d-dim space)
3:36
• Let a, b ∈ Qfree and γ a path in Qfree connecting a and b.
Then the probability that P RM found the path after n samples

Rapidly Exploring Random Trees
is
2|γ| −σ̺d n 2 motivations:

P (PRM-success | n) ≥ 1 − e
̺
|B1 |
• Single Query path finding: Focus computational efforts on paths
σ= 2d |Qfree |
for specific (qstart , qgoal )
̺ = clearance of γ (distance to obstacles)
(roughly: the exponential term are “volume ratios”)
• Use actually controllable DoFs to incrementally explore the search
space: control-based path finding.
• This result is called probabilistic complete (one can achieve any
probability with high enough n)
(Ensures that RRTs can be extended to handling differential
• For a given success probability, n needs to be exponential in d
constraints.)
3:34
3:37
Other PRM sampling strategies

n = 1 n = 100 n = 300 n = 600 n = 1000 n = 2000
3:38
Simplest RRT with straight line local planner and step size α
Input: qstart , number n of nodes, stepsize α

Output: tree T = (V, E)
1: initialize V = {qstart }, E = ∅
2: for i = 0 : n do
3: qtarget ← random sample from Q
4: qnear ← nearest neighbor of qtarget in V
α
5: qnew ← qnear + |q −q
(q
| target
− qnear )
target near
6: if qnew ∈ Qfree then V ← V ∪ {qnew }, E ← E ∪
{(qnear , qnew )}
7: end for
3:39
RRT growing directedly towards the goal
Input: qstart , qgoal , number n of nodes, stepsize α, β

2: for i = 0 : n do
3: if rand(0, 1) < β then qtarget ← qgoal
4: else qtarget ← random sample from Q
α
6: qnew ← qnear + |q −q
(q
| target
− qnear )
target near
{(qnear , qnew )}
8: end for
3:40
n = 1 n = 100 n = 200 n = 300 n = 400 n = 500

3:41
Bi-directional search
• grow two trees starting from qstart and qgoal
let one tree grow towards the other

(e.g., “choose qnew of T1 as qtarget of T2 ”)
3:42
Summary: RRTs
• Pros (shared with PRMs):

– Algorithmically very simple
– Highly explorative
– Allows probabilistic performance guarantees
• Pros (beyond PRMs):

– Focus computation on single query (qstart , qgoal ) problem
– Trees from multiple queries can be merged to a roadmap
– Can be extended to differential constraints (nonholonomic sys-
tems)
• To keep in mind (shared with PRMs):

– The metric (for nearest neighbor selection) is sometimes criti-
cal
– The local planner may be non-trivial
3:43
References Car example
Steven M. LaValle: Planning Algorithms, http://planning. • The car is a non-holonomic system: not all DoFs are controlled,
cs.uiuc.edu/. dim(u) < dim(q)
We have the differential constraint q̇:
Choset et. al.: Principles of Motion Planning, MIT Press.

ẋ sin θ − ẏ cos θ = 0
Latombe’s “motion planning” lecture, http://robotics.stanford. “A car cannot move directly lateral.”
edu/˜latombe/cs326/2007/schedule.htm
3:44 • Analogy to dynamic systems: Just like a car cannot instantly move side-
wards, a dynamic system cannot instantly change its position q: the
current change in position is constrained by the current velocity q̇.
Non-holonomic systems 3:48
3:45
Path finding with a non-holonomic system
Non-holonomic systems Could a car follow this trajectory?
• We define a nonholonomic system as one with differential

constraints:
dim(ut ) < dim(xt )
⇒ Not all degrees of freedom are directly controllable
• Dynamic systems are an example!
• General definition of a differential constraint:

For any given state x, let Ux be the tangent space that is gener-
ated by controls: This is generated with a PRM in the state space q = (x, y, θ)
Ux = {ẋ : ẋ = f (x, u), u ∈ U } ignoring the differntial constraint.
(non-holonomic ⇐⇒ dim(Ux ) < dim(x)) 3:49
The elements of Ux are elements of Tx subject to additional Path finding with a non-holonomic system
differential constraints.
This is a solution we would like to have:
3:46
Car example
ẋ = v cos θ
ẏ = v sin θ
θ̇ = (v/L) tan ϕ
|ϕ| < Φ
 
x   The path respects differential constraints.

State q = y 

 Controls u =  v
  
 ϕ Each step in the path corresponds to setting certain controls.
θ
   
ẋ
   v cos θ  3:50
   
Sytem equation  ẏ  =  v sin θ 
   
   
θ̇ (v/L) tan ϕ
3:47 Control-based sampling to grow a tree
• Control-based sampling: fulfils none of the nice exploration prop-

erties of RRTs, but fulfils the differential constraints:
1) Select a q ∈ T from tree of current configurations
2) Pick control vector u at random
3) Integrate equation of motion over short duration (picked at

random or not)
4) If the motion is collision-free, add the endpoint to the tree
3:51
car parking
3:54
Control-based sampling for the car
1) Select a q ∈ T
2) Pick v, φ, and τ
3) Integrate motion from q
4) Add result if collision-
free
3:52
parking with only left-steering

3:55
J. Barraquand and J.C. Latombe. Nonholonomic Multibody Robots:

Controllability and Motion Planning in the Presence of Obstacles.
Algorithmica, 10:121-155, 1993.
car parking with a trailer

3:53 3:56
Better control-based exploration: RRTs re-

visited
• RRTs with differential constraints:
Input: qstart , number k of nodes, time interval τ

2: for i = 0 : k do
3: qtarget ← random sample from Q
5: use local planner to compute controls u that steer qnear
towards qtarget Rτ
6: qnew ← qnear + t=0 q̇(q, u)dt
{(qnear , qnew )}
8: end for → By testing all six types of trajectories for (q1 , q2 ) we can define
a Dubins metric for the RRT – and use the Dubins curves as
• Crucial questions: controls!
– How meassure near in nonholonomic systems?

– How find controls u to steer towards target? • Reeds-Shepp curves are an extension for cars which can drive
3:57
back.
(includes 46 types of trajectories, good metric for use in RRTs
for cars)
Metrics
3:60
Standard/Naive metrics:
• Comparing two 2D rotations/orientations θ1 , θ2 ∈ SO(2):
a) Euclidean metric between eiθ1 and eiθ2
b) d(θ1 , θ2 ) = min{|θ1 − θ2 |, 2π − |θ1 − θ2 |}
• Comparing two configurations (x, y, θ)1,2 in R2 :
Eucledian metric on (x, y, eiθ )
• Comparing two 3D rotations/orientations r1 , r2 ∈ SO(3):
Represent both orientations as unit-length quaternions r1 , r2 ∈ R4 :
d(r1 , d2 ) = min{|r1 − r2 |, |r1 + r2 |}
where | · | is the Euclidean metric.
(Recall that r1 and −r1 represent exactly the same rotation.)
• Ideal metric:
Optimal cost-to-go between two states x1 and x2 :
– Use optimal trajectory cost as metric
– This is as hard to compute as the original problem, of course!!
→ Approximate, e.g., by neglecting obstacles.
3:58
Dubins curves
• Dubins car: constant velocity, and steer ϕ ∈ [−Φ, Φ]
• Neglecting obstacles, there are only six types of trajectories that

connect any configuration ∈ R2 × S1 :
{LRL, RLR, LSL, LSR, RSL, RSR}
• annotating durations of each phase:

{Lα Rβ Lγ , , Rα Lβ Rγ , Lα Sd Lγ , Lα Sd Rγ , Rα Sd Lγ , Rα Sd Rγ }
with α ∈ [0, 2π), β ∈ (π, 2π), d ≥ 0
3:59
Dubins curves
4 Path Optimization • Alternativ: formulate hard constraints in the framework of con-

strained optimization
very briefly 4:4
Outline
• These are only some very brief notes on path optimization
• The aim is to explain how to formulate the optimization prob-

lem. Concerning the optimization algortihm itself, refer to the
Optimization lecture.
4:1
From inverse kinematics to path costs
• Recall our optimality principle of inverse kinematics
argmin ||q − q0 ||2W + ||Φ(q)||2

q
• A trajectory q0:T is a sequence of robot configurations qt ∈ Rn

• Consider the cost function
T
X T
X
f (q0:T ) = ||Ψt (qt-k , .., qt )||2 + ||Φt (qt )||2
t=0 t=0
(where (q−k , .., q-1 ) is a given prefix)
• Ψt (qt-k , .., qt ) represents control costs

k denotes the order of the control costs
Φt (qt ) represents task costs
(More generally, task costs could depend on Φt (qt-k , .., qt ))
4:2
Control costs
• The Ψt (qt-k , .., qt ) can penalize various things:
k=0 Ψt (qt ) = qt − q0 penalize offset from

zero
k=1 Ψt (qt-1 , qt ) = qt − qt-1 penalize velocity
k=2 Ψt (qt-2 , .., qt ) = qt − 2qt-1 + qt-2 penalize acceleration
k=3 Ψt (qt-3 , .., qt ) = qt − 3qt-1 + 3qt-2 − qt-3 penalize jerk
• The big Φt (qt ) imposes tasks as for inverse kinematics

4:3
Choice of optimizer
T
X T
X
f (q0:T ) = ||Ψt (qt-k , .., qt )||2 + ||Φt (qt )||2
t=0 t=0
Is in the form of the so-called Gauss-Newton optimization prob-

lem, and can be solved using such 2nd order methods.
(Note that the pseudo Hessian is a banded, symmetric, positive-definite matrix.)
5 Dynamics – The notation xt refers to the dynamic state of the system: e.g.,
joint positions and velocities xt = (qt , q̇t ).
1D point mass, damping & oscillation, PID, dynamics of mechanical
– f is an arbitrary function, often smooth
systems, Euler-Lagrange equation, Newton-Euler recursion, general
5:3
robot dynamics, joint space control, reference trajectory following,
operational space control
Outline
• We start by discussing a 1D point mass for 3 reasons:

Kinematic Dynamic – The most basic force-controlled system with inertia
– We can introduce and understand PID control
instantly change joint velocities instantly change joint torques – The behavior of a point mass under PID control is a refer-
q̇: u: ence that we can also follow with arbitrary dynamic robots
! ! (if the dynamics are known)
δqt = J ♯ (y ∗ − φ(qt )) u=?
accounts for kinematic cou- accounts for dynamic coupling • We discuss computing the dynamics of general robotic systems
pling of joints but ignores in- of joints and full Newtonian – Euler-Lagrange equations
ertia, forces, torques physics – Euler-Newton method
• We derive the dynamic equivalent of inverse kinematics:
gears, stiff, all of industrial future robots, compliant, few – operational space control
robots research robots 5:4
PID and a 1D point mass
5:5
5:1
The dynamics of a 1D point mass
When velocities cannot be changed/set arbi-
• Start with simplest possible example: 1D point mass
trarily
(no gravity, no friction, just a single mass)
• Examples:
– An air plane flying: You cannot command it to hold still in the
air, or to move straight up.
– A car: you cannot command it to move side-wards.
• The state x(t) = (q(t), q̇(t)) is described by:
– Your arm: you cannot command it to throw a ball with arbitrary
– position q(t) ∈ R
speed (force limits).
– velocity q̇(t) ∈ R
– A torque controlled robot: You cannot command it to instantly
change velocity (infinite acceleration/torque).
• The controls u(t) is the force we apply on the mass point
• What all examples have in comment:

• The system dynamics is:
– One can set controls ut (air plane’s control stick, car’s steer-
ing wheel, your muscles activations, torque/voltage/current send
q̈(t) = u(t)/m
to a robot’s motors)
– But these controls only indirectly influence the dynamics of
5:6
state, xt+1 = f (xt , ut )
5:2
1D point mass – proportional feedback
Dynamics • Assume current position is q.
The goal is to move it to the position q ∗ .
• The dynamics of a system describes how the controls ut influ-
ence the change-of-state of the system
xt+1 = f (xt , ut ) What can we do?
• Idea 1: q̇ ∗ is a desired goal velocity

“Always pull the mass towards the goal q ∗ :” For simplicity we set q̇ ∗ = 0 in the following.
u = Kp (q ∗ − q)
5:7 5:10
1D point mass – proportional feedback 1D point mass – derivative feedback
• What’s the effect of this control law? • What’s the effect of this control law?
mq̈ = u = Kp (q ∗ − q) + Kd (0 − q̇)
m q̈ = u = Kp (q ∗ − q)
• Solution: again assume q(t) = a + beωt

q = q(t) is a function of time, this is a second order differential
equation m b ω 2 eωt = Kp q ∗ − Kp a − Kp b eωt − Kd b ωeωt
• Solution: assume q(t) = a + beωt (m b ω 2 + Kd b ω + Kp b) eωt = Kp (q ∗ − a)
(a “non-imaginary” alternative would be q(t) = a+b ǫ−λt cos(ωt)) ⇒ (m ω 2 + Kd ω + Kp ) = 0 ∧ (q ∗ − a) = 0
p
−Kd ± Kd2 − 4mKp
m b ω 2 eωt = Kp q ∗ − Kp a − Kp b eωt ⇒ω=
2m
(m b ω 2 + Kp b) eωt = Kp (q ∗ − a) q(t) = q ∗ + b eω t
⇒ (m b ω 2 + Kp b) = 0 ∧ (q ∗ − a) = 0
p The term − K
2m
d
in ω is real ↔ exponential decay (damping)
⇒ ω = i Kp /m 5:11
√
q(t) = q ∗ + b ei Kp /m t
1D point mass – derivative feedback
This is an oscillation around q ∗ with amplitude b = q(0) − q ∗ and
p p
frequency Kp /m! −Kd ± Kd2 − 4mKp
∗ ω t
q(t) = q + b e , ω=
5:8 2m
1D point mass – proportional feedback p

• Effect of the second term Kd2 − 4mKp /2m in ω:
Kd2 < 4mKp ⇒ ω has imaginary part p

∗
m q̈ = u = Kp (q − q) oscillating with frequency Kp /m − Kd2 /4m2
√
√ 2 2
q(t) = q ∗ + be−Kd /2m t ei Kp /m−Kd /4m t
q(t) = q ∗ + b ei Kp /m t
Kd2 > 4mKp ⇒ ω real
strongly damped
Oscillation around q ∗ with amplitude b = q(0)−q ∗ and frequency Kd2 = 4mKp ⇒ second term zero
p
Kp /m only exponential decay
5:12

1 cos(x)
0.5
-0.5
-1
0 2 4 6 8 10 12 14
5:9
• Idea 2
“Pull less, when we’re heading the right direction already:”
“Damp the system:” illustration from O. Brock’s lecture
5:13
u = Kp (q ∗ − q) + Kd (q̇ ∗ − q̇)
1D point mass – derivative feedback Controlling a 1D point mass – lessons learnt
Alternative parameterization: • Proportional and derivative feedback (PD control) are like adding
Instead of the gains Kp and Kd it is sometimes more intuitive to a spring and damper to the point mass
set the
• PD control is a linear control law

• wave length λ = 1
ω0
= √ 1
, Kp = m/λ2
Kp /m
(q, q̇) 7→ u = Kp (q ∗ − q) + Kd (q̇ ∗ − q̇)

• damping ratio ξ = √ Kd = λKd
2m
, Kd = 2mξ/λ
4mKp
(linear in the dynamic system state x = (q, q̇))
ξ > 1: over-damped
• With such linear control laws we can design approach trajecto-
ξ = 1: critically dampled
ries (by tuning the gains)
ξ < 1: oscillatory-damped
– but no optimality principle behind such motions
√ 5:17
1−ξ2 t/λ
q(t) = q ∗ + be−ξ t/λ ei
5:14 Dynamics of mechanical systems
1D point mass – integral feedback 5:18
• Idea 3
Two ways to derive dynamics equations for
“Pull if the position error accumulated large in the past:” mechanical systems
Z t • The Euler-Lagrange equation

u = Kp (q ∗ − q) + Kd (q̇ ∗ − q̇) + Ki (q ∗ (s) − q(s)) ds
s=0 d ∂L ∂L
− =u
dt ∂ q̇ ∂q
• This is not a linear ODE w.r.t. x = (q, q̇).
However, when we extend the state to x = (q, q̇, e) we have the ODE Used when you want to derive analytic equations of motion (“on paper”)
q̇ = q̇
∗ ∗
q̈ = u/m = Kp /m(q − q) + Kd /m(q̇ − q̇) + Ki /m e
∗ • The Newton-Euler recursion (and related algorithms)
ė = q − q
(no explicit discussion here) fi = mv̇i , ui = Ii ẇ + w × Iw

5:15
Algorithms that “propagate” forces through a kinematic tree and numer-
ically compute the inverse dynamics u = NE(q, q̇, q̈) or forward dynam-
1D point mass – PID control ics q̈ = f (q, q̇, u).
5:19
Z t
u = Kp (q ∗ − q) + Kd (q̇ ∗ − q̇) + Ki (q ∗ − q(s)) ds The Euler-Lagrange equation
s=0
d ∂L ∂L
• PID control − =u
dt ∂ q̇ ∂q
– Proportional Control (“Position Control”)
• L(q, q̇) is called Lagrangian and defined as
f ∝ Kp (q ∗ − q)
L=T −U
– Derivative Control (“Damping”)
f ∝ Kd (q̇ ∗ − q̇) (ẋ∗ = 0 → damping) where T =kinetic energy and U =potential energy.
• q is called generalized coordinate – any coordinates such that
(q, q̇) describes the state of the system. Joint angles in our case.
– Integral Control (“Steady State Error”)
Rt • u are external forces
f ∝ Ki s=0 (q ∗ (s) − q(s)) ds
5:20
5:16
The Euler-Lagrange equation – Euler’s equation expresses the torque (=control!) act-
ing on a rigid body given an angular velocity and angular
• How is this typically done? acceleration:
• First, describe the kinematics and Jacobians for every link i: ui = Ii ẇ + w × Iw
(q, q̇) 7→ {TW →i (q), vi , wi }
Recall TW →i (q) = TW →A TA→A′ (q) TA′ →B TB→B ′ (q) · · ·

• Forward recursion: (≈ kinematics)
Further, we know that a link’s velocity vi = Ji q̇ can be described via its position
Jacobian.
Compute (angular) velocities (vi , wi ) and accelerations (v̇i , ẇi )
Similarly we can describe the link’s angular velocity wi = Jiw q̇ as linear in q̇. for every link (via forward propagation; see geometry notes for details)
• Second, formulate the kinetic energy
X1 X1 ⊤    
1
T = mi vi2 + w⊤
i I i wi = q̇⊤Mi q̇ , Mi =  Jw
i   m i I3 0  Ji  • Backward recursion:
2 2 2 Ji 0 Ii Jiw
i i
For the leaf links, we now know the desired accelerations q ∗ and
where Ii = Ri I¯i R⊤ ¯
i and Ii the inertia tensor in link coordinates
can compute the necessary joint torques. Recurse backward.
• Third, formulate the potential energies (typically independent of q̇) 5:23
U = gmi height(i)
Numeric algorithms for forward and inverse
• Fourth, compute the partial derivatives analytically to get something like dynamics
d ∂L ∂L ∂T ∂U
u = − = M q̈ + Ṁ q̇ − + • Newton-Euler recursion: very fast (O(n)) method to compute
|{z} dt ∂ q̇ ∂q |{z} ∂q ∂q
control inertia | {z } |{z}
Coriolis gravity
inverse dynamics
u = NE(q, q̇, q̈ ∗ )
which relates accelerations q̈ to the forces
Note that we can use this algorithm to also compute
5:21
– gravity terms: u = NE(q, 0, 0) = G(q)
– Coriolis terms: u = NE(q, q̇, 0) = C(q, q̇) q̇ + G(q)
Example: A pendulum – column of Intertia matrix: u = NE(q, 0, ei ) = M (q) ei
• Articulated-Body-Dynamics: fast method (O(n)) to compute

forward dynamics q̈ = f (q, q̇, u)
5:24
• Generalized coordinates: angle q = (θ)

Some last practical comments
• Kinematics:
– velocity of the mass: v = (lθ̇ cos θ, 0, lθ̇ sin θ)
• [demo]
– angular velocity of the mass: w = (0, −θ̇, 0)
• Use energy conservation to measure dynamic of physical simu-
• Energies:
lation
1 1 1 • Physical simulation engines (developed for games):
T = mv 2 + w⊤Iw = (ml2 + I2 )θ̇2 , U = −mgl cos θ
2 2 2 – ODE (Open Dynamics Engine)
– Bullet (originally focussed on collision only)
• Euler-Lagrange equation:
– Physx (Nvidia)
d ∂L ∂L
u= − Differences of these engines to Lagrange, NE or ABD:
dt ∂ q̇ ∂q
– Game engine can model much more: Contacts, tissues,
d
= (ml2 + I2 )θ̇ + mgl sin θ = (ml2 + I2 )θ̈ + mgl sin θ particles, fog, etc
dt
– (The way they model contacts looks ok but is somewhat
5:22
fictional)
– On kinematic trees, NE or ABD are much more precise than
game engines
Newton-Euler recursion – Game engines do not provide inverse dynamics, u = NE(q, q̇, q̈)
• An algorithm that computes the inverse dynamics

• Proper modelling of contacts is really really hard
u = NE(q, q̇, q̈ ∗ ) 5:25
by recursively computing force balance at each joint:

Dynamic control of a robot
– Newton’s equation expresses the force acting at the cen-
ter of mass for an accelerated body:
5:26
fi = mv̇i
• Choose a desired acceleration q̈t∗ that implies a PD-like behavior

around the reference trajectory!
• We previously learnt the effect of PID control on a 1D point mass
q̈t∗ = q̈tref + Kp (qtref − qt ) + Kd (q̇tref − q̇t )
This is a standard and very convenient heuristic to track a reference
• Robots are not a 1D point mass trajectory when the robot dynamics are known: All joints will exactly
behave like a 1D point particle around the reference trajectory!
– Neither is each joint a 1D point mass
5:30
– Applying separate PD control in each joint neglects force
coupling
(Poor solution: Apply very high gains separately in each Controlling a robot – operational space ap-
joint ↔ make joints stiff, as with gears.)
proach
• However, knowing the robot dynamics we can transfer our un- • Recall the inverse kinematics problem:
derstanding of PID control of a point mass to general systems – We know the desired step δy ∗ (or velocity ẏ ∗ ) of the endeffec-
5:27 tor.
General robot dynamics – Which step δq (or velocities q̇) should we make in the joints?
• Let (q, q̇) be the dynamic state and u ∈ Rn the controls (typically
• Equivalent dynamic problem:
joint torques in each motor) of a robot
– We know how the desired acceleration ÿ ∗ of the endeffector.
• Robot dynamics can generally be written as:
– What controls u should we apply?
M (q) q̈ + C(q, q̇) q̇ + G(q) = u 5:31
M (q) ∈ Rn×n is positive definite intertia matrix

(can be inverted → forward simulation of dynamics) Operational space control
C(q, q̇) ∈ Rn are the centripetal and coriolis forces
G(q) ∈ Rn are the gravitational forces
u are the joint torques
(cf. to the Euler-Lagrange equation on slide 22)
• Inverse kinematics:
• We often write more compactly:
M (q) q̈ + F (q, q̇) = u q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W
q
5:28
Controlling a general robot

• Operational space control (one might call it “Inverse task space
• From now on we jsut assume that we have algorithms to effi- dynamics”):
ciently compute M (q) and F (q, q̇) for any (q, q̇)
u∗ = argmin ||φ̈(q) − ÿ ∗ ||2C + ||u||2H
• Inverse dynamics: If we know the desired q̈ ∗ for each joint, u
u = M (q) q̈ ∗ + F (q, q̇) 5:32
gives the necessary torques

Operational space control
• Forward dynamics: If we know which torques u we apply, use
q̈ ∗ = M (q)-1 (u − F (q, q̇))

• We can derive the optimum perfectly analogous to inverse kine-
to simulate the dynamics of the system (e.g., using Runge-Kutta) matics
5:29 We identify the minimum of a locally squared potential, using the local
linearization (and approx. J¨ = 0)
Controlling a general robot – joint space ap-
d d ˙ ≈ J q̈ + 2J˙q̇ = JM -1 (u − F ) + 2J˙q̇
proach φ̈(q) =
dt
φ̇(q) ≈
dt
(J q̇ + Jq)
• Where could we get the desired q̈ ∗ from? We get

ref
Assume we have a nice smooth reference trajectory q0:T (gen-
u∗ = T ♯ (ÿ ∗ − 2J˙q̇ + T F )
erated with some motion profile or alike), we can at each t read
off the desired acceleration as with T = JM -1 , T ♯ = (T⊤CT + H)-1 T⊤C
1
q̈tref := [(qt+1 − qt )/τ − (qt − qt-1 )/τ ] = (qt-1 + qt+1 − 2qt )/τ 2 (C → ∞ ⇒ T ♯ = H -1 T⊤(T H -1 T⊤)-1 )
τ
5:33
However, tiny errors in acceleration will accumulate greatly over
time! This is Instable!!
Controlling a robot – operational space ap-

proach
• Where could we get the desired ÿ ∗ from?

ref
– Reference trajectory y0:T in operational space
– PD-like behavior in each operational space:
ÿt∗ = ÿtref + Kp (ytref − yt ) + Kd (ẏtref − ẏt )
illustration from O. Brock’s lecture
• Operational space control: Let the system behave as if we could

directly “apply a 1D point mass behavior” to the endeffector
5:34
Multiple tasks
• Recall trick last time: we defined a “big kinematic map” Φ(q)

such that
q ∗ = argmin ||q − q0 ||2W + ||Φ(q)||2
q
• Works analogously in the dynamic case:
u∗ = argmin ||u||2H + ||Φ(q)||2

u
5:35
6 Probability Basics Probabilities & Random Variables
• For a random variable X with discrete domain dom(X) = Ω we

write:
Probability Theory ∀x∈Ω : 0 ≤ P (X = x) ≤ 1
P
x∈Ω P (X = x) = 1
• Why do we need probabilities?
Example: A dice can take values Ω = {1, .., 6}.

– Obvious: to express inherent stochasticity of the world (data) X is the random variable of a dice throw.
P (X = 1) ∈ [0, 1] is the probability that X takes value 1.
• But beyond this: (also in a “deterministic world”):

• A bit more formally: a random variable relates a measureable space with
– lack of knowledge! a domain (sample space) and thereby introduces a probability measure
on the domain (“assigns a probability to each possible value”)
– hidden (latent) variables
6:4
– expressing uncertainty
– expressing information (and lack of information) Probabilty Distributions
• P (X = 1) ∈ R denotes a specific probability

• Probability Theory: an information calculus
P (X) denotes the probability distribution (function over Ω)
6:1
Example: A dice can take values Ω = {1, 2, 3, 4, 5, 6}.

Probability: Frequentist and Bayesian
By P (X) we discribe the full distribution over possible values {1, .., 6}.
These are 6 numbers that sum to one, usually stored in a table, e.g.:
• Frequentist probabilities are defined in the limit of an infinite [ 16 , 61 , 61 , 61 , 16 , 61 ]
number of trials
Example: “The probability of a particular coin landing heads up is 0.43”
• In implementations we typically represent distributions over dis-
crete random variables as tables (arrays) of numbers
• Bayesian (subjective) probabilities quantify degrees of belief
Example: “The probability of it raining tomorrow is 0.3”
– Not possible to repeat “tomorrow” • Notation for summing over a RV:
In equation we often needP to sum over RVs. We then write
6:2
X P (X) · · ·
P
as shorthand for the explicit notation x∈dom(X) P (X = x) · · ·
Probabilities & Sets 6:5
• Sample Space/domain Ω, e.g. Ω = {1, 2, 3, 4, 5, 6}

Joint distributions
• Probability P : A ⊂ Ω 7→ [0, 1] Assume we have two random variables X and Y
e.g., P ({1}) = 61 , P ({4}) = 16 , P ({2, 5}) = 31 ,

• Definitions:
• Axioms: ∀A, B ⊆ Ω Joint: P (X, Y )

P
– Nonnegativity P (A) ≥ 0 Marginal: P (X) = Y P (X, Y )
P (X,Y )
– Additivity P (A ∪ B) = P (A) + P (B) if A ∩ B = ∅ Conditional: P (X|Y ) = P (Y )
– Normalization P (Ω) = 1
P
The conditional is normalized: ∀Y : X P (X|Y ) = 1
• Implications
0 ≤ P (A) ≤ 1 • X is independent of Y iff: P (X|Y ) = P (X)
(table thinking: all columns of P (X|Y ) are equal)
P (∅) = 0
A ⊆ B ⇒ P (A) ≤ P (B)
• The same for n random variables X1:n (stored as a rank n tensor)
P (A ∪ B) = P (A) + P (B) − P (A ∩ B) P
Joint: P (x1:n ), Marginal: P (X1 ) = X2:n P (X1:n ),
P (X1:n )
P (Ω \ A) = 1 − P (A) Conditional: P (X1 |X2:n ) = P (X2:n )
6:3 6:6
Joint distributions Gaussian distribution

joint: P (X, Y ) 1 2
/σ 2
P • 1-dim: N(x | µ, σ 2 ) = 1
|2πσ 2 |1/2
e− 2 (x−µ)
marginal: P (X) = Y P (X, Y )
N (x|µ, σ 2 )
P (X,Y )
conditional: P (X|Y ) = P (Y )
2σ
• Implications of these definitions: µ x
1 ⊤
A-1 (x−a)
• n-dim: N(x | a, A) = 1
|2πA|1/2
e− 2 (x−a)
Product rule: P (X, Y ) = P (X|Y ) P (Y ) = P (Y |X) P (X)
x2
P (Y |X) P (X)
Bayes’ Theorem P (X|Y ) = P (Y ) x1
(b)
Useful identities:
Symmetry: N(x|a, A) = N(a|x, A) = N(x − a|0, A)
• The same for n variables, e.g., (X, Y, Z): Product:
N(x | a, A) N(x | b, B) = N(x | B(A+B)-1 a+A(A+B)-1 b, A(A+B)-1 B) N(a | b, A
B)
P (X, Z, Y ) =
Qn “Propagation”:
P (X1:n ) = i=1 P (Xi |Xi+1:n ) P (X|Y, Z) P (Y |Z) P (Z) R
y
N(x | a + F y, A) N(y | b, B) dy = N(x | a + F b, A + F BF⊤)
P (X1 |X2:n ) = P (Y |X,Z) P (X|Z)
P (X|Y, Z) = P (Y |Z) Transformation:
P (X2 |X1 ,X3:n ) P (X1 |X3:n ) N(F x + f | a, A) = 1
N(x | F -1 (a − f ), F -1 AF -⊤ )
|F |
P (X2 |X3:n ) P (X,Z|Y ) P (Y )
P (X, Y |Z) = P (Z)
6:7 Mre identities: see “Gaussian identities” http://userpage.fu-berlin.de/
˜mtoussai/notes/gaussians.pdf
6:10
Bayes’ Theorem
Particle Approximation of a Distribution
• We approximate a distribution p(x) over a continuous domain

P (Y |X) P (X)
P (X|Y ) = Rn .
P (Y )
• A particle distribution q(x) is a weighed set of N particles {(xi , wi )}N
i=
likelihood · prior – each particle has a location xi ∈ Rn and a weight wi ∈ R

posterior = normalization P
– weights are normalized i wi = 1
6:8
PN
Distributions over continuous domain q(x) := i=1 wi δ(x − xi )
• Let X be a continuous RV. The probability density function

where δ(x − xi ) is the δ-distribution.
(pdf) p(x) ∈ [0, ∞) defines the probability
6:11
Z b
P (a ≤ X ≤ b) = p(x) dx ∈ [0, 1]
a Particle Approximation of a Distribution
The (cumulative) probability distribution F (x) = P (X ≤
Rx Histogram of a particle representation:
x) = −∞ dx p(x) ∈ [0, 1] is the cumulative integral with limx→∞ F (x) =
1.
(In discrete domain: probability distribution and probability mass func-

tion P (X) ∈ [0, 1] are used synonymously.)
• Two basic examples:

1 ⊤
A-1 (xa)
Gaussian: N(x | a, A) = 1
|2πA|1/2
e− 2 (xa)
R
Dirac or δ (“point particle”) δ(x) = 0 except at x = 0, δ(x) dx =
1
∂
δ(x) = ∂x
H(x) where H(x) = [x ≥ 0] = Heavyside step func-
tion.
6:12
6:9
Particle Approximation of a Distribution
• For q(x) to approximate a given p(x) we want to choose parti-

cles and weights such that for any (smooth) f :
PN R
limN →∞ hf (x)iq = limN →∞ i=1 wi f (xi ) = x
f (x)p(x)dx = hf (x)ip
• How to do this? See An Introduction to MCMC for Machine

Learning www.cs.ubc.ca/˜nando/papers/mlintro.pdf
6:13
Some continuous distributions

1 ⊤
A-1 (xa)
Gaussian N(x | a, A) = 1
|2πA|1/2
e− 2 (xa)
∂
Dirac or δ δ(x) = ∂x H(x)
2 ν+1
Student’s t p(x; ν) ∝ [1 + xν ]− 2
(=Gaussian for ν → ∞, otherwise
heavy tails)
Exponential p(x; λ) = [x ≥ 0] λe−λx
(distribution over single event time)
1 −|x−µ|/b
Laplace p(x; µ, b) = 2b
e
(“double exponential”)
Chi-squared p(x; k) ∝ [x ≥ 0] xk/2−1 e−x/2
Gamma p(x; k, θ) ∝ [x ≥ 0] xk−1 e−x/θ
6:14
7 Mobile Robotics
State estimation, Bayes filter, odometry, particle filter, Kalman filter,
SLAM, joint Bayes filter, EKF SLAM, particle SLAM, graph-based
SLAM
http://stair.stanford.edu/multimedia.php
STAIR: STanford Artificial Intelligence Robot

7:4
Types of Robot Mobility

http://www.darpa.mil/grandchallenge05/
DARPA Grand Challenge 2005

7:1
7:5
Types of Robot Mobility
http://www.darpa.mil/grandchallenge/index.asp
DARPA Grand Urban Challenge 2007

7:2
7:6
http://www.slawomir.de/publications/grzonka09icra/
grzonka09icra.pdf
Quadcopter Indoor Localization • Each type of robot mobility corresponds to a

7:3 system equation xt+1 = xt + τ f (xt , ut )
or, if the dynamics are stochastic,
P (xt+1 | ut , xt ) = N(xt+1 | xt + τ f (xt , ut ), Σ) Recall Bayes’ theorem
• We considered control, path finding, and trajectory optimization

P (Y |X) P (X)
P (X|Y ) = P (Y )
For this we always assumed to know the state xt of the robot
(e.g., its posture/position)!
likelihood · prior
7:7 posterior = (normalization)
7:11
Outline
• PART I: • How can we apply this to the
A core challenge in mobile robotics is state estimation State Estimation Problem?
→ Bayesian filtering & smoothing

particles, Kalman
• PART II: Using Bayes Rule:

P (sensor | location)P (location)
Another challenge is to build a map while exploring P (location | sensor) = P (sensor)
→ SLAM (simultaneous localization and mapping)

7:8
PART I: State Estimation Problem
• Our sensory data does not provide sufficient information to de-

7:12
termine our location.
Bayes Filter
• Given the local sensor readings yt , the current state xt (location,
position) is uncertain. xt = state (location) at time t
yt = sensor readings at time t
– which hallway? ut-1 = control command (action, steering, velocity) at time t-1
– which door exactly?

– which heading direction? • Given the history y0:t and u0:t-1 , we want to compute the proba-
bility distribution over the state at time t
7:9
pt (xt ) := P (xt | y0:t , u0:t-1 )
• Generally:
State Estimation Problem xt
Filtering: P (xt |y0:t ) 111111111
000000000
000000000
111111111
y 0:t
x t
Smoothing: P (xt |y0:T ) 000000000000000
111111111111111
000000000000000
111111111111111
y 0:T
• What is the probability of being in
xt
front of room 154, given we see Prediction: P (xt |y0:s ) 11111
00000
00000
11111
y 0:s
what is shown in the image?

7:13
• What is the probability given that

we were just in front of room 156? Bayes Filter
• What is the probability given that

we were in front of room 156 and pt (xt ) := P (xt | y0:t , u0:t-1 )
moved 15 meters?
= ct P (yt | xt , y0:t-1 , u0:t-1 ) P (xt | y0:t-1 , u0:t-1 )
7:10 = ct P (yt | xt ) P (xt | y0:t-1 , u0:t-1 )
Z
= ct P (yt | xt ) P (xt , xt-1 | y0:t-1 , u0:t-1 ) dxt-1
xt-1
Z
= ct P (yt | xt ) P (xt | xt-1 , y0:t-1 , u0:t-1 ) P (xt-1 | y0:t-1 , u0:t-1 ) dxt-1 • The predictive distributions p̂t (xt ) without integrating observa-
xt-1
Z tions (removing the P (yt |xt ) part from the Bayesian filter)
= ct P (yt | xt ) P (xt | xt-1 , ut-1 ) P (xt-1 | y0:t-1 , u0:t-1 ) dxt-1
xt-1
Z
= ct P (yt | xt ) P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1
xt-1
using Bayes rule P (X|Y, Z) = c P (Y |X, Z) P (X|Z) with some

normalization constant ct
uses conditional independence of the observation on past ob-
servations and controls
by definition of the marginal
by definition of a conditional (from Robust Monte Carlo localization for mobile robots Sebas-
tian Thrun, Dieter Fox, Wolfram Burgard, Frank Dellaert)
given xt-1 , xt depends only on the controls ut-1 (Markov Prop-
7:17
erty)
• A Bayes filter updates the posterior belief pt (xt ) in each time
step using the:
Again, predictive distributions p̂t (xt ) without integrating land-
observation model P (yt | xt )
mark observations
transition model P (xt | ut-1 , xt-1 )
7:14
Bayes Filter
Z
pt (xt ) ∝ P (yt | xt ) P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1
| {z } xt-1 | {z }
new information old estimate
| {z }
predictive estimate p̂t (xt )
1. We have a belief pt-1 (xt-1 ) of our previous position

7:18
2. We use the motion model to predict the current position

R
p̂t (xt ) ∝ xt-1 P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1
The Bayes-filtered distributions pt (xt ) integrating landmark ob-
servations
3. We integetrate this with the current observation to get a better
belief
pt (xt ) ∝ P (yt | xt ) p̂t (xt )
7:15
• Typical transition model P (xt | ut-1 , xt-1 ) in robotics:
7:19
Bayesian Filters
• How to represent the belief pt (xt ):

(from Robust Monte Carlo localization for mobile robots Sebas-
tian Thrun, Dieter Fox, Wolfram Burgard, Frank Dellaert)
7:16
• Gaussian
Odometry (“Dead Reckoning”)

aka Monte Carlo Localization in the mobile robotics community
• Particles Condensation Algorithm in the vision community
• Efficient resampling is important:

Typically “Residual Resampling”:
Instead of sampling directly n̂i ∼ Multi({N wi }) set n̂i = ⌊N wi ⌋ + n̄i

with n̄i ∼ Multi({N wi − ⌊N wi ⌋})
7:20 Liu & Chen (1998): Sequential Monte Carlo Methods for Dynamic Sys-
tems.
Douc, Cappé & Moulines: Comparison of Resampling Schemes for Par-
Recall: Particle Representation of a Distribu- ticle Filtering.
tion 7:23
• Weighed set of N particles {(xi , wi )}N

i=1
Example: Quadcopter Localization
PN
p(x) ≈ q(x) := i=1 wi δ(x, xi )
http://www.slawomir.de/publications/grzonka09icra/
grzonka09icra.pdf
Quadcopter Indoor Localization

7:24
7:21 Typical Pitfall in Particle Filtering
Particle Filter := Bayesian Filtering with Par- • Predicted particles {xit }N

i=1 have very low observation likelihood
ticles P (yt | xit ) ≈ 0
R (“particles die over time”)

(Bayes Filter: pt (xt ) ∝ P (yt | xt ) xt-1
P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1 )
• Classical solution: generate particles also with other than purely

forward proposal P (xt | ut-1 , xt-1 ):
– Choose a proposal that depends on the new observation yt ,

ideally approximating P (xt | yt , ut-1 , xt-1 )
– Or mix particles sampled directly from P (yt | xt ) and from

P (xt | ut-1 , xt-1 ).
1. Start with N particles {(xit-1 , wt-1
i
)}N
i=1
(Robust Monte Carlo localization for mobile robots. Sebastian Thrun, Dieter Fox,
2. Resample particles to get N weight-1-particles: {x̂it-1 }N
i=1
Wolfram Burgard, Frank Dellaert)
3. Use motion model to get new “predictive” particles {xit }N
i=1
each xit ∼ P (xt | ut-1 , x̂it-1 ) 7:25
4. Use observation model to assign new weights wti ∝ P (yt | xit )

7:22 Kalman filter := Bayesian Filtering with Gaus-
sians
R
• “Particle Filter” Bayes Filter: pt (xt ) ∝ P (yt | xt ) xt-1
P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1
• Can be computed analytically for linear-Gaussian observations Bayes smoothing

and transitions:
P (yt | xt ) = N(yt | Cxt + c, W ) xt
Filtering: P (xt |y0:t ) 111111111
000000000
000000000
111111111
y 0:t
P (xt | ut-1 , xt-1 ) = N(xt | A(ut-1 ) xt-1 + a(ut-1 ), Q)
x t
Smoothing: P (xt |y0:T ) 000000000000000
111111111111111
000000000000000
111111111111111
y 0:T
Defition:
N(x | a, A) = 1
exp{− 12 (x - a)⊤ A-1 (x - a)} x
|2πA|1/2 t
Prediction: P (xt |y0:s ) 00000
11111
00000
11111
Product: y 0:s
N(x | a, A) N(x | b, B) = N(x | B(A+B)-1 a+A(A+B)-1 b, A(A+B)-1 B) N(a | b, A+
B) 7:29
“Propagation”:
R
y
N(x | a + F y, A) N(y | b, B) dy = N(x | a + F b, A + F BF⊤)
Transformation: Bayes smoothing
N(F x + f | a, A) = 1
|F |
N(x | F -1 (a − f ), F -1 AF -⊤ )
(more identities: see “Gaussian identities” http://ipvs.informatik.uni-stuttgart. • Let P = y0:t past observations, F = yt+1:T future observations
de/mlr/marc/notes/gaussians.pdf)
7:26 P (xt | P, F, u0:T ) ∝ P (F | xt , P, u0:T ) P (xt | P, u0:T )

= P (F | xt , ut:T ) P (xt | P, u0:t-1 )
Kalman filter derivation | {z } | {z }
=:βt (xt ) =:p(xt )
Bayesian smoothing fuses a forward filter pt (xt ) with a backward “filter”

pt (xt ) = N(xt | st , St )
P (yt | xt ) = N(yt | Cxt + c, W ) βt (xt )
P (xt | ut-1 , xt-1 ) = N(xt | Axt-1 + a, Q)
Z
pt (xt ) ∝ P (yt | xt ) P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1
xt-1 • Backward recursion (derivation analogous to the Bayesian filter)
Z
= N(yt | Cxt + c, W ) N(xt | Axt-1 + a, Q) N(xt-1 | st-1 , St-1 ) dxt-1
xt-1
βt (xt ) := P (yt+1:T | xt , ut:T )
⊤
= N(yt | Cxt + c, W ) N(xt | Ast-1 + a, Q + ASt-1 A ) Z
| {z } | {z }
=:ŝt =:Ŝt
= βt+1 (xt+1 ) P (yt+1 | xt+1 ) P (xt+1 | xt , ut ) dxt+1
xt+1
= N(Cxt + c | yt , W ) N(xt | ŝt , Ŝt )
⊤ -1 ⊤ -1
= N[xt | C W (yt − c), C W C] N(xt | ŝt , Ŝt ) 7:30
= N(xt | st , St ) · hterms indep. of xt i
⊤ -1 -1 -1 ⊤ ⊤ -1
St = (C W C + Ŝt ) = Ŝt − Ŝt C (W + C Ŝt C ) C Ŝt
| {z } PART II: Localization and Mapping
“Kalman gain” K
⊤ -1 -1
st = St [C W (yt − c) + Ŝt ŝt ] = ŝt + K(yt − C ŝt − c)
• The Bayesian filter requires an observation model P (yt | xt )
The second to last line uses the general Woodbury identity.
The last line uses St C⊤W -1 = K and St Ŝt-1 = I − KC
• A map is something that provides the observation model:
7:27
A map tells us for each xt what the sensor readings yt might
Extended Kalman filter (EKF) and Unscented look like
Transform 7:31
R
Bayes Filter: pt (xt ) ∝ P (yt | xt ) xt-1
P (xt | ut-1 , xt-1 ) pt-1 (xt-1 ) dxt-1 Types of maps
• Can be computed analytically for linear-Gaussian observations Grid map

Landmark map
and transitions:
P (yt | xt ) = N(yt | Cxt + c, W )
P (xt | ut-1 , xt-1 ) = N(xt | A(ut-1 )xt-1 + a(ut-1 ), Q)
• If P (yt | xt ) or P (xt | ut-1 , xt-1 ) are not linear: K. Murphy (1999): Bayesian map learning
in dynamic environments.
Grisetti, Tipaldi, Stachniss, Burgard, Nardi:
P (yt | xt ) = N(yt | g(xt ), W ) Fast and Accurate SLAM with
Rao-Blackwellized Particle Filters
P (xt | ut-1 , xt-1 ) = N(xt | f (xt-1 , ut-1 ), Q)
Victoria Park data set
– approximate f and g as locally linear (Extended Kalman Filter ) Laser scan map M. Montemerlo, S. Thrun, D. Koller, & B.
Wegbreit (2003): FastSLAM 2.0: An im-
– or sample locally from them and reapproximate as Gaussian proved particle filtering algorithm for simul-
taneous localization and mapping that prov-
(Unscented Transform) ably converges. IJCAI, 1151–1156.
7:28 7:32
Simultaneous Localization and Mapping Prob- Map uncertainty

lem
• In the case the map m = (θ1 , .., θN ) is a set of N landmarks,
• Notation: θj ∈ R 2
xt = state (location) at time t
yt = sensor readings at time t
ut-1 = control command (action, steering, velocity) at time t-1
m = the map
• Given the history y0:t and u0:t-1 , we want to compute the belief
over the pose AND THE MAP m at time t
pt (xt , m) := P (xt , m | y0:t , u0:t-1 )
• We assume to know: • Use Gaussians to represent the uncertainty of landmark posi-

– transition model P (xt | ut-1 , xt-1 ) tions
– observation model P (yt | xt , m) 7:36
7:33
(Extended) Kalman Filter SLAM

SLAM: classical “chicken or egg problem”
• Analogous to Localization with Gaussian for the pose belief pt (xt )
• If we knew the state trajectory x0:t we could efficiently compute
– But now: joint belief pt (xt , θ1:N ) is 3 + 2N -dimensional Gaus-
the belief over the map
sian
– Assumes the map m = (θ1 , .., θN ) is a set of N landmarks,
P (m | x0:t , y0:t , u0:t-1 )
θj ∈ R 2
– Exact update equations (under the Gaussian assumption)
• If we knew the map we could use a Bayes filter to compute the – Conceptually very simple
belief over the state
• Drawbacks:
P (xt | m, y0:t , u0:t-1 )
– Scaling (full covariance matrix is O(N 2 ))
– Sometimes non-robust (uni-modal, “data association problem”)
• SLAM requires to tie state estimation and map building together: – Lacks advantages of Particle Filter
1) Joint inference on xt and m (→ Kalman-SLAM) (multiple hypothesis, more robust to non-linearities)

7:37
2) Tie a state hypothesis (=particle) to a map hypothesis
(→ particle SLAM)
3) Frame everything as a graph optimization problem (→ graph
SLAM with particles
SLAM)
Core idea: Each particle carries its own map belief
7:34
Joint Bayesian Filter over x and m • Use a conditional representation “pt (xt , m) = pt (xt ) pt (m | xt )”
(This notation is flaky... the below is more precise)
• A (formally) straight-forward approach is the joint Bayesian filter
R • As for the Localization Problem use particles to represent the

pt (xt , m) ∝ P (yt | xt , m) P (xt | ut-1 , xt-1 ) pt-1 (xt-1 , m) dxt-1
xt-1 pose belief pt (xt )
Note: Each particle actually “has a history xi0:t ” – a whole tra-
jectory!
But: How represent a belief over high-dimensional xt , m?
7:35
• For each particle separately, estimate the map belief pit (m) con-
ditioned on the particle history xi0:t .
The conditional beliefs pit (m) may be factorized over grid points Particle-based SLAM summary
or landmarks of the map
• We have a set of N particles {(xi , wi )}N
i=1 to represent the pose
belief pt (xt )
K. Murphy (1999): Bayesian map learning in dynamic environ-
ments.
• For each particle we have a separate map belief pit (m); in the
http://www.cs.ubc.ca/˜murphyk/Papers/map_nips99.pdf
case of landmarks, this factorizes in N separate 2D-Gaussians
7:38
• Iterate
1. Resample particles to get N weight-1-particles: {x̂it-1 }N
i=1
Map estimation for a given particle history
2. Use motion model to get new “predictive” particles {xit }N
i=1
• Given x0:t (e.g. a trajectory of a particle), what is the posterior 3. Update the map belief pim (m) ∝ P (yt | m, xt ) pit-1 (m) for each
over the map m? particle
R
4. Compute new importance weights wti ∝ P (yt | xit , m) pt−1 (m) d
→ simplified Bayes Filter: using the observation model and the map belief
7:41
pt (m) := P (m | x0:t , y0:t ) ∝ P (yt | m, xt ) pt-1 (m)

Demo: Visual SLAM
(no transtion model: assumption that map is constant) • Map building from a freely moving camera
• In the case of landmarks (FastSLAM):

m = (θ1 , .., θN )
θj = position of the jth landmark, j ∈ {1, .., N }
nt = which landmark we observe at time t, nt ∈ {1, .., N }
We can use a separate (Gaussian) Bayes Filter for each θj

conditioned on x0:t , each θj is independent from each θk :
Y
P (θ1:N | x0:t , y0:n , n0:t ) = P (θj | x0:t , y0:n , n0:t )
j
7:39
7:42
Particle likelihood in SLAM Demo: Visual SLAM
• Particle likelihood for Localization Problem: • Map building from a freely moving camera
wti = P (yt | xit )

(determins the new importance weight wti – SLAM has become a big topic in the vision community..
– features are typically landmarks θ1:N with SURF/SIFT features
• In SLAM the map is uncertain → each particle is weighted with – PTAM (Parallel Tracking and Mapping) parallelizes computa-
the expected likelihood: tions...
R
wti = P (yt | xit , m) pt−1 (m) dm
PTAM1 PTAM2 DTAM KinectFusion
• In case of landmarks (FastSLAM):

R
wti = P (yt | xit , θnt , nt ) pt−1 (θnt ) dθnt G Klein, D Murray: Parallel Tracking and Mapping for Small AR
Workspaces http://www.robots.ox.ac.uk/˜gk/PTAM/
• Data association problem (actually we don’t know nt ): Newcombe, Lovegrove & Davison: DTAM: Dense Tracking and
For each particle separately choose nit = argmaxnt wti (nt ) Mapping in Real-Time ICCV 2011. http:
7:40 //www.doc.ic.ac.uk/˜rnewcomb/
7:43
Alternative SLAM approach: Graph-based

• Visual SLAM
e.g. http://ewokrampage.wordpress.com/
7:47
• Represent the previous trajectory as a graph

– nodes = estimated positions & observations
– edges = transition & step estimation based on scan matching
• Loop Closing: check if some nodes might coincide → new edges
• Classical Optimization:
The whole graph defines an optimization problem: Find poses
that minimize sum of edge & node errors
7:44
Loop Closing Problem
(Doesn’t explicitly exist in Particle Filter methods: If particles

cover the belief, then “data association” solves the “loop closing
problem”)
7:45
Graph-based SLAM
Life-long Map Learning for Graph-based SLAM Approaches in Static Environ-
ments Kretzschmar, Grisetti, Stachniss
7:46
SLAM code
• Graph-based and grid map methods:

http://openslam.org/
8 Control Theory • Transfer function

Analyze the closed-loop transfer function, i.e., “how frequencies are
transmitted through the system”. (→ Laplace transformation)
Topics in control theory, optimal control, HJB equation, infinite hori-
zon case, Linear-Quadratic optimal control, Riccati equations (dif- • Controller design
Find a controller with desired stability and/or transfer function properties
ferential, algebraic, discrete-time), controllability, stability, eigenvalue
analysis, Lyapunov function • Optimal control*
Define a cost function on the system behavior. Optimize a controller to
minimize costs
Cart pole example 8:3
Control Theory references

θ
u • Robert F. Stengel: Optimal control and estimation

Online lectures: http://www.princeton.edu/˜stengel/
x MAE546Lectures.html (esp. lectures 3,4 and 7-9)
• From robotics lectures:

Stefan Schaal’s lecture Introduction to Robotics: http://www-clm
state x = (x, ẋ, θ, θ̇)
h i usc.edu/Teaching/TeachingIntroductionToRoboticsSyl
g sin(θ) + cos(θ) −c1 u − c2 θ̇2 sin(θ) Drew Bagnell’s lecture on Adaptive Control and Reinforcement
θ̈ = 4
3
l − c2 cos2 (θ) Learning http://robotwhisperer.org/acrls11/
h i
ẍ = c1 u + c2 θ̇2 sin(θ) − θ̈ cos(θ) 8:4
8:1 Outline
Control Theory • We’ll first consider optimal control

Goal: understand Algebraic Riccati equation
• Concerns controlled systems of the form significance for local neighborhood control
ẋ = f (x, u) + noise(x, u) • Then controllability & stability

8:5
and a controller of the form
π : (x, t) 7→ u Optimal control (discrete time)
Given a controlled dynamic system
• We’ll neglect stochasticity here xt+1 = f (xt , ut )
we define a cost function

• When analyzing a given controller π, one analyzes closed-loop
system as described by the differential equation T
X
Jπ = c(xt , ut ) + φ(xT )
t=0
ẋ = f (x, π(x, t))
where x0 and the controller π : (x, t) 7→ u are given, which
(E.g., analysis for convergence & stability) determines x1:T and u0:T
8:2 8:6
Core topics in Control Theory Dynamic Programming & Bellman principle
• Stability* An optimal policy has the property that whatever the initial state
Analyze the stability of a closed-loop system and initial decision are, the remaining decisions must constitute
→ Eigenvalue analysis or Lyapunov function method
an optimal policy with regard to the state resulting from the first
• Controllability* decision.
Analyze which dimensions (DoFs) of the system can actually in principle
be controlled
1 1
1 3
Derivation:
3
7 1
15 Goal dV (x, t) ∂V ∂V
5 = + ẋ
Start 8 20 3 dt ∂t ∂x
3 3 ∂V ∂V
10 ∗
c(x, u ) = +
∗
f (x, u )
1 ∂t ∂x
1 ∂V ∂V
∗ ∗
5 3 − = c(x, u ) + f (x, u )
∂t ∂x
8:10
“V (state) = minedge [c(edge) + V (next-state)]”
8:7 Infinite horizon case
Bellman equation (discrete time) Z ∞

Jπ = c(x(t), u(t)) dt
0
• Define the value function or optimal cost-to-go function
• This cost function is stationary (time-invariant)!
T
hX i
Vt (x) = min c(xs , us ) + φ(xT ) → the optimal value function is stationary (V (x, t) = V (x))
π xt =x
s=t
→ the optimal control signal depends on x but not on t
• Bellman equation → the optimal controller π ∗ is stationary
h i
Vt (x) = minu c(x, u) + Vt+1 (f (x, u))
h i • The HBJ and Bellman equations remain “the same” but with the
The argmin gives the optimal control signal: πt∗ (x) = argminu · · ·
same (stationary) value function independent of t:
h ∂V i
Derivation:
0 = min c(x, u) + f (x, u) (cont. time)
u ∂x
hX
T i h i
Vt (x) = min
π
c(xs , us ) + φ(xT ) V (x) = min c(x, u) + V (f (x, u)) (discrete time)
s=t u
h T
X i h i
= min c(x, ut ) + min[ c(xs , us ) + φ(xT )]
ut π
s=t+1
The argmin gives the optimal control signal: π ∗ (x) = argminu · · ·
h i
= min c(x, ut ) + Vt+1 (f (x, ut )) 8:11
ut
8:8 Infinite horizon examples
Optimal Control (continuous time) • Cart-pole balancing:

– You always want the pole to be upright (θ ≈ 0)
– You always want the car to be close to zero (x ≈ 0)
Given a controlled dynamic system
– You want to spare energy (apply low torques) (u ≈ 0)
You might define a cost
ẋ = f (x, u)
Z ∞
Jπ = ||θ||2 + ǫ||x||2 + ρ||u||2
we define a cost function with horizon T 0
Z T
• Reference following:
Jπ = c(x(t), u(t)) dt + φ(x(T ))
0 – You always want to stay close to a reference trajectory r(t)
˙
Define x̃(t) = x(t) − r(t) with dynamics x̃(t) = f (x̃(t) + r(t), u) − ṙ(t)
where the start state x(0) and the controller π : (x, t) 7→ u Define a cost Z ∞
are given, which determine the closed-loop system trajectory Jπ = ||x̃||2 + ρ||u||2
0
x(t), u(t) via ẋ = f (x, π(x, t)) and u(t) = π(x(t), t)
• Many many problems in control can be framed this way
8:9
8:12
Hamilton-Jacobi-Bellman equation (continu-

Comments
ous time)
• The Bellman equation is fundamental in optimal control theory,
• Define the value function or optimal cost-to-go function
but also Reinforcement Learning
hZ T i
V (x, t) = min c(x(s), u(s)) ds + φ(x(T )) • The HJB eq. is a differential equation for V (x, t) which is in gen-
π t x(t)=x
eral hard to solve
• Hamilton-Jacobi-Bellman equation • The (time-discretized) Bellman equation can be solved by Dy-
h i
∂ ∂V
− ∂t V (x, t) = minu c(x, u) + ∂x
f (x, u) namic Programming starting backward:
h i h i
The argmin gives the optimal control signal: π ∗ (x) = argminu · · · VT (x) = φ(x) , VT -1 (x) = min c(x, u) + VT (f (x, u)) etc.
u
But it might still be hard or infeasible to represent the functions 8:16
Vt (x) over continuous x!

Riccati differential equation
• Both become significantly simpler under linear dynamics and
quadratic costs: −Ṗ = A⊤P + P A − P BR-1 B⊤P + Q
→ Riccati equation
8:13
• This is a differential equation for the matrix P (t) describing the
quadratic value function. If we solve it with the finite horizon
Linear-Quadratic Optimal Control constraint P (T ) = F we solved the optimal control problem
linear dynamics
• The optimal control u∗ = −R-1 B⊤P x is called Linear Quadratic
ẋ = f (x, u) = Ax + Bu Regulator
quadratic costs
Note: If the state is dynamic (e.g., x = (q, q̇)) this control is linear
c(x, u) = x⊤Qx + u⊤Ru , φ(xT ) = x⊤T F xT
in the positions and linear in the velocities and is an instance of
• Note: Dynamics neglects constant term; costs neglect linear PD control
and constant terms. This is because The matrix K = R-1 B⊤P is therefore also called gain matrix
For instance, if x(t) = (q(t) − r(t), q̇(t) − ṙ(t)) for a reference r(t) and
– constant costs are irrelevant K = [ Kp Kd ] then
– linear cost terms can be made away by redefining x or u
u∗ = Kp (r(t) − q(t)) + Kd (ṙ(t) − q̇(t))
– constant dynamic term only introduces a constant drift
8:14 8:17
Linear-Quadratic Control as Local Approxi- Riccati equations

mation
• Finite horizon continuous time
• LQ control is important also to control non-LQ systems in the Riccati differential equation
neighborhood of a desired state!
−Ṗ = A⊤P + P A − P BR-1 B⊤P + Q , P (T ) = F
Let x∗ be such a desired state (e.g., cart-pole: x∗ = (0, 0, 0, 0))

– linearize the dynamics around x∗
• Infinite horizon continuous time
– use 2nd order approximation of the costs around x∗
Algebraic Riccati equation (ARE)
– control the system locally as if it was LQ
0 = A⊤P + P A − P BR-1 B⊤P + Q
– pray that the system will never leave this neighborhood!
8:15 PT
• Finite horizon discrete time (J π = t=0 ||xt ||2Q +||ut ||2R +||xT ||2F )
Riccati differential equation = HJB eq. in LQ
Pt-1 = Q + A⊤[Pt − Pt B(R + B⊤Pt B)-1 B⊤Pt ]A , PT = F
case
• In the Linear-Quadratic (LQ) case, the value function always is

P∞
a quadratic function of x! • Infinite horizon discrete time (J π = t=0 ||xt ||2Q + ||ut ||2R )
P = Q + A⊤[P − P B(R + B⊤P B)-1 B⊤P ]A

Let V (x, t) = x⊤P (t)x, then the HBJ equation becomes
∂ h ∂V i 8:18
− V (x, t) = min c(x, u) + f (x, u)
∂t u ∂x
h i
−x⊤Ṗ (t)x = min x⊤Qx + u⊤Ru + 2x⊤P (t)(Ax + Bu) Example: 1D point mass
u
∂ h ⊤ i
0= x Qx + u⊤Ru + 2x⊤P (t)(Ax + Bu) • Dynamics:
∂u
= 2u⊤R + 2x⊤P (t)B q̈(t) = u(t)/m
u∗ = −R-1 B⊤P x          
x= q q̇   q̇  =  0 1 x +  0  u
  ,   = 
ẋ =      
q̇ q̈ u(t)/m 0 0 1/m
⇒ Riccati differential equation    
= Ax + Bu , A= 0 1 , B= 0 
   
⊤ ⊤ 0 0 1/m
−Ṗ = A P + P A − P BR B P + Q
-1
• Costs: • Trajectory Optimization: (control hard constraints could be included)
T T
c(x, u) = ǫ||x||2 + ̺||u||2 , Q = ǫI , R = ̺I X X
min f (q0:T ) = ||Ψt (qt-k , .., qt )||2 + ||Φt (qt )||2
q0:T
t=0 t=0
• Algebraic Riccati equation:
  • Reinforcement Learning:
−1
P = a 

c ,

∗
u = −R B P x = -1 ⊤
[cq + bq̇]
c b ̺m – Markov Decision Processes ↔ discrete time stochastic con-
⊤
0 = A P + P A − P BR B P + Q -1 ⊤ trolled system P (xt+1 | ut , xt )
       
= c b  + 0 a  − 1  c2 bc  + ǫ 1 0 – Bellman equation → Basic RL methods (Q-learning, etc)

       
0 0 0 c ̺m2 bc b2 0 1
8:22
8:19
Example: 1D point mass (cont.)

Controllability
• Algebraic Riccati equation: 8:23
 
a c , −1
P =
  u∗ = −R-1 B⊤P x = [cq + bq̇]
c b ̺m Controllability
       
0= c b  + 0 a  − 1  c2 bc  + ǫ 1 0
       
0 0 0 c ̺m2 bc b2 0 1 • As a starting point, consider the claim:
√ √ “Intelligence means to gain maximal controllability over all de-
First solve for c, then for b = m ̺ c + ǫ. Whether the damping
ration ξ = √b depends on the choices of ̺ and ǫ. grees of freedom over the environment.”
4mc
• The Algebraic Riccati equation is usually solved numerically. Note:

(E.g. are, care or dare in Octave) – controllability (ability to control) 6= control
8:20 – What does controllability mean exactly?
Optimal control comments

• I think the general idea of controllability is really interesting
• HJB or Bellman equation are very powerful concepts – Linear control theory provides one specific definition of con-
trollability, which we introduce next..
• Even if we can solve the HJB eq. and have the optimal control, 8:24
we still don’t know how the system really behaves?

– Will it actually reach a “desired state”?
Controllability
– Will it be stable? • Consider a linear controlled system
– It is actually “controllable” at all?
ẋ = Ax + Bu
• Last note on optimal control:
Formulate as a constrainted optimization problem with objective function How can we tell from the matrices A and B whether we can
J π and constraint ẋ = f (x, u). λ(t) are the Langrange multipliers. It
∂
turns out that ∂x V (x, t) = λ(t). (See Stengel.) control x to eventually reach any desired state?
8:21
• Example: x is 2-dim, u is 1-dim:

Relation to other topics
      
 ẋ1  =  0 0  x 1  +  1 u
• Optimal Control: 
ẋ2
 
0

0 x2
  
0
Z T
min J π = c(x(t), u(t)) dt + φ(x(T )) Is x “controllable”?
π 0
• Inverse Kinematics:
      
 ẋ1  =  0 1  x 1  +  0 u
min f (q) = ||q − q0 ||2W + ||φ(q) − y ∗ ||2C 
ẋ2
 
0

0 x2
  
1
q
• Operational space control: Is x “controllable”?

8:25
min f (u) = ||u||2H + ||φ̈(q) − ÿ ∗ ||2C

u
Controllability 8:29
We consider a linear stationary (=time-invariant) controlled sys-

Stability
tem
ẋ = Ax + Bu 8:30
• Complete controllability: All elements of the state can be brought

from arbitrary initial conditions to zero in finite time Stability
• A system is completely controllable iff the controllability matrix
• One of the most central topics in control theory
h i
C := B AB A2 B · · · An-1 B
has full rank dim(x) (that is, all rows are linearly independent) • Instead of designing a controller by first designing a cost func-
tion and then applying Riccati,
• Meaning of C:
design a controller such that the desired state is provably a sta-
The ith row describes how the ith element xi can be influenced by u
“B”: ẋi is directly influenced via B ble equilibrium point of the closed loop system
“AB”: ẍi is “indirectly” influenced via AB (u directly influences some ẋj 8:31
via B; the dynamics A then influence ẍi depending on ẋj )
...
“A2 B”: x i is “double-indirectly” influenced
etc... Stability
Note: ẍ = Aẋ + B u̇ = AAx + ABu + B u̇
...
x = A3 x + A2 Bu + AB u̇ + B ü
• Stability is an analysis of the closed loop system. That is: for
8:26
this analysis we don’t need to distinguish between system and
Controllability controller explicitly. Both together define the dynamics
• When all rows of the controllability matrix are linearly indepen- ẋ = f (x, π(x, t)) =: f (x)
dent ⇒ (u, u̇, ü, ...) can influence all elements of x indepen-
dently • The following will therefore discuss stability analysis of general
• If a row is zero → this element of x cannot be controlled at all differential equations ẋ = f (x)
• If 2 rows are linearly dependent → these two elements of x will
remain coupled forever • What follows:
8:27
– 3 basic definitions of stability
Controllability examples – 2 basic methods for analysis by Lyapunov

8:32
        
 ẋ1  =  0 0   x 1  +  1 u C= 1 0  rows linearly dependent
        
ẋ2 0 0 x2 1 1 0
        
 ẋ1  =  0 0   x 1  +  1 u C= 1 0  2nd row zero
        
ẋ2 0 0 x2 0 0 0
        
 ẋ1  =  0 1  x 1  +  0 u C= 0 1  good!
        
ẋ2 0 0 x2 1 1 0
8:28
Controllability
Why is it important/interesting to analyze controllability?
• The Algebraic Riccati Equation will always return an “optimal”

controller – but controllability tells us whether such a controller Aleksandr Lyapunov (1857–1918)
even has a chance to control x
8:33
• “Intelligence means to gain maximal controllability over all de- Stability – 3 definitions
grees of freedom over the environment.”
– real environments are non-linear ẋ = F (x) with equilibrium point x = 0
– “to have the ability to gain controllability over the environment’s • x0 is an equilibrium point ⇐⇒ f (x0 ) = 0
DoFs”
• Lyapunov stable or uniformly stable ⇐⇒ 8:36
∀ǫ : ∃δ s.t. ||x(0)|| ≤ δ ⇒ ||x(t)|| ≤ ǫ

Side note: Stability for discrete time systems
(when it starts off δ-near to x0 , it will remain ǫ-near forever) • Given a discrete time linear system
xt+1 = Axt
• asymtotically stable ⇐⇒ Let λi be the eigenvalues of A

Lyapunov stable and limt→∞ x(t) = 0
– The system is asymptotically stable ⇐⇒ ∀i : |λi | < 1
– The system is unstable stable ⇐⇒ ∃i : |λi | > 1
– The system is marginally stable ⇐⇒ ∀i : |λi | ≤ 1
• exponentially stable ⇐⇒ 8:37
asymtotically stable and ∃α, a s.t. ||x(t)|| ≤ ae−αt ||x(0)||
R∞
(→ the “error” time integral 0 ||x(t)||dt ≤ αa ||x(0)|| is bounded!)
Linear Stability Analysis comments
8:34
• The same type of analysis can be done locally for non-linear

Linear Stability Analysis
systems, as we will do for the cart-pole in the exercises
(“Linear” ↔ “local” for a system linearized at the equilibrium
point.) • We can design a controller that minimizes the (negative) eigen-
• Given a linear system values of A:
ẋ = Ax ↔ controller with fastest asymtopic convergence
Let λi be the eigenvalues of A
– The system is asymptotically stable ⇐⇒ ∀i : real(λi ) < 0 This is a real alternative to optimal control!
– The system is unstable stable ⇐⇒ ∃i : real(λi ) > 0 8:38
– The system is marginally stable ⇐⇒ ∀i : real(λi ) ≤ 0

Lyapunov function method
• Meaning: An eigenvalue describes how the system behaves along one
state dimension (along the eigenvector):
• A method to analyze/prove stability for general non-linear sys-
ẋi = λi xi
tems is the famous “Lyapunov’s second method”
As for the 1D point mass the solution is xi (t) = aeλi t and
– imaginary λi → oscillation
– negative real(λi ) → exponential decay ∝ e−|λi |t Let D be a region around the equilibrium point x0
– positive real(λi ) → exponential explosion ∝ e|λi |t
• A Lyaponov function V (x) for a system dynamics ẋ = f (x) is
8:35
– positive, V (x) > 0, everywhere in D except...
Linear Stability Analysis: Example at the equilibrium point where V (x0 ) = 0
∂V (x)
– always decreases, V̇ (x) = ∂x
ẋ < 0, in D except...
• Let’s take the 1D point mass q̈ = u/m in closed loop with a PD
u = −Kp q − Kd q̇ at the equilibrium point where f (x) = 0 and therefore V̇ (x) = 0
• Dynamics:
     
q̇   0 1 x + 1/m 0 0 x • If there exists a D and a Lyapunov function ⇒ the system is
  = 
ẋ =    
q̈ 0 0 −Kp −Kd asymtotically stable
 
A= 0 1 
 
−Kp /m −Kd /m
If D is the whole state space, the system is globally stable
• Eigenvalues:
     8:39
The equation λ q 0 1  q 
  =    leads to the

q̇ −Kp /m −Kd /m q̇
equation λq̇ = λ q = −Kp /mq −Kd /mλq or mλ2 +Kd λ+Kp =
2
Lyapunov function method
0 with solution (compare slide 05:10)
p
−Kd ± Kd2 − 4mKp • The Lyapunov function method is very general. V (x) could be
λ=
2m “anything” (energy, cost-to-go, whatever). Whenever one finds
For Kd2 − 4mKp negative, the real(λ) = −Kd /2m some V (x) that decreases, this proves stability
⇒ Positive derivative gain Kd makes the system stable.
• The problem though is to think of some V (x) given a dynamics! • Adaptive Control: When system dynamics ẋ = f (x, u, β) has
(In that sense, the Lyapunov function method is rather a method unknown parameters β.
of proof than a concrete method for stability analysis.) – One approach is to estimate β from the data so far and use
optimal control.
• In standard cases, a good guess for the Lyapunov function is – Another is to design a controller that has an additional internal
either the energy or the value function update equation for an estimate β̂ and is provably stable. (See
8:40 Schaal’s lecture, for instance.)
8:43
Lyapunov function method – Energy Exam-
ple
• Let’s take the 1D point mass q̈ = u/m in closed loop with a PD

u = −Kp q − Kd q̇, which has the solution (slide 05:14):
√
2
q(t) = be−ξ/λ t eiω0 1−ξ t
• Energy of the 1D point mass: V (q, q̇) := 21 mq̇ 2
V̇ (x) = e−2ξ/λ t V (x(0))
(using that the energy of an undamped oscillator is conserved)

• V (x) < 0 ⇐⇒ ξ > 0 ⇐⇒ Kd > 0
Same result as for the eigenvalue analysis
8:41
Lyapunov function method – value function

Example
• Consider infinite horizon linear-quadratic optimal control. The

solution of the Algebraic Riccati equation gives the optimal con-
troller.
• The value function satisfies
V (x) = x⊤P x
V̇ (x) = [Ax + Bu∗ ]⊤P x + x⊤P [Ax + Bu∗ ]
u∗ = −R-1 B⊤P x = Kx
V̇ (x) = x⊤[(A + BK)⊤P + P (A + BK)]x
= x⊤[A⊤P + P A + (BK)⊤P + P (BK)]x
0 = A⊤P + P A − P BR-1 B⊤P + Q
V̇ (x) = x⊤[P BR-1 B⊤P − Q + (P BK)⊤ + P BK]x
= −x⊤[Q + K⊤RK]x
(We could have derived this easier! x⊤Qx are the immediate state costs,
and x⊤K⊤RKx = u⊤Ru are the immediate control costs—and V̇ (x) =
−c(x, u∗ )! See slide 11 bottom.)
• That is: V is a Lyapunov function if Q + K⊤RK is positive defi-

nite!
8:42
Observability & Adaptive Control
• When some state dimensions are not directly observable: ana-

lyzing higher order derivatives to infer them.
Very closely related to controllability: Just like the controllabil-
ity matrix tells whether state dimensions can (indirectly) be con-
trolled; an observation matrix tells whether state dimensions can
(indirectly) be inferred.
9 Practical: A 2-wheeled Racer 2D Modelling
A 2-wheeled racer
• Educational ideas:
– have a really dynamic system
– have a system which, in the “racing”
limit, is hard to control • See theoretical modelling notes
– learn about hardware, communica-
9:6
tion, etc
– challenges connecting theory with
practise: 3D Modelling
• Real world issues:
– control interface (“setting velocities”) • Account for centrifugal forces in a curve
is adventurous
• Generalized coordinates q = (x, y, φ, θ), with steering angle φ
– PARTIAL OBSERVABILITY: we only
have a noisy accelerometer & gyro-
scope
• Exercise: Derive general Euler-Lagrange equations
– unknown time delays
9:7
– unknown system parameters
(masses, geometry, etc)
9:1 Clash of theory and real world
9:8
Intro
The control interface
9:2
• Theory assumed torque control

• In real, the motor controller “does things somehow”. We can
[demo] set:
∗
9:3 – a target velocities vl,r
– desired acceleration level a∗l,r ∈ {−10, .., −1, 1, .., 10}
• The controller will then ramp velocity in 25msec steps depend-
Components
ing on a∗ until target v ∗ is reached
• Odroid: on-board PC running xubuntu • Unknown: time delays, scaling of a∗ ?
• Motor unit: motors, motor driver, motor controller, Hall sensor
• IMU (inertial measurement unit): 3D accelerometer, 3D gyro- • Potential approach:
scope, (magnetic) – Assume acceleration control interface
– Consider constrained Euler-Lagrange equations
• Communication: USB-to-I2C communicates with both, motors
9:9
and IMU
Coping with the partial observability

• See Marcel’s thesis
9:4 • Theoretical view: In LQG systems it is known that optimal con-
trol under partial observability is the same as optimal control
assuming the Bayes estimated state as true current state. Un-
Code
certainty principle.
• From Marcel’s thesis: • Use a Bayes filter to estimate the state (q, q̇) from all sensor
– Control loop (around 36 msec) information we have
• Sensor information:
• Kalman filter tests on the accelerometer: – Accelerometer readings ãx,y,z
– Caroline – Gyro readings g̃x,y,z
9:5
– Motor positions θ̃l,r . Note that θ̃ ∝ x/r − θ desribes the
relative angle between the pole and the wheels
• Open issue: time delays – relevant?
9:10 • Local linearization (x = (q, q̇))
q̈ = Ax + a + B̄u
Coping with unknown system parameters
∂
A= M -1 (Bu − F ) , B̄ = M -1 B
∂x
• System identification
→ gradient check
• We derived the eqs of motion Bu = M q̈ + F (for 2D) – but don’t
know the parameters → Riccati eqn → nice controller [demo]
– mA , IA , mB , IB : masses and inertias of bodies A (=wheel) 9:15
and B (=pendulum)
– r: radius of the wheel Modelling overview II
– l: length of the pendulum (height of its COM)
• Sensor model
 
y acc = c1 R [p̈B − (0, g)⊤] , R= cos(θ + c2 ) − sin(θ + c2 ) 

• Focus on the local linearization around (q, q̇) = 0  
sin(θ + c2 ) cos(θ + c2 )
• OR: Use blackbox optimization to fit parameters to data y gyro = c3 (θ̇ + c4 )
9:11
y enc = c5 (x/r − θ)
y = (y acc , y gyro , y enc ) ∈ R4
Data
• We need data to understand better what’s going on!

• Local linearization
∂y ∂y ∂ q̈
• Lot’s of data of full control cycles around (q, q̇) = 0 C= = ∂y
∂q
∂y
∂ q̇
+
∂(q, q̇) ∂ q̈ ∂(q, q̇)
(sensor reading, control signals, cycle time)
→ gradient check
→ Kalman filtering [demo]
• Data specifically on how motors accelerate when setting a de- 9:16
sired acceleration level
9:12 Modelling overview III
Or completely different: Reinforcement Learn- • Constrained Euler-Lagange equations for acceleration control
– Our motors actually don’t allow to set torques – but rather
ing set accelerations. Setting accelerations implies the con-
straint
• Alternatively one fully avoids modelling → Reinforcement Learn- B ′ q̈ = u′
ing – Using q̈ = M -1 (Bu − F ) we can retrieve the torque
u = (B ′ M -1 B)-1 [u′ + B ′ M -1 F ]
• Roughly: blackbox optimization (e.g., EA) of PD parameters
that exactly generates this acceleration
9:13 – Plugging this back into q̈ = M -1 (Bu − F ) we get
q̈ = B ′# u′ − (I − B ′# B ′ )M -1 F , B ′# = M -1 B(B ′ M -1 B)-1
Modelling
9:17
9:14
Modelling summary
Modelling overview I
• We now have all analytic models we need
We have exact analytical models (and implemented) for the fol-
• In simulation we have no problem to apply
lowing: – Riccati to retrieve a (locally) optimal linear regulator
– Kalman to optimally (subject to linearizations) estimate the
state
• Euler-Lagange equations
M (q) q̈ + F (q, q̇) = B(q) u • The crux: we have 12 unknown parameters

-1
q̈ = M (Bu − F )
mA , IA , m B , IB , r, l, lC , c1 , .., c5
→ energy check (plus sensor noise parameters σa , σg , σe )

→ physical simulation 9:18
System Identification
9:19
System Identification
• Given data D = {(x, u, y)t }Tt=1 , learn

Deisenroth, Rasmussen & Fox: Learning to Control a Low-Cost Manip-
ulator using Data-Efficient Reinforcement Learning (RSS 2011)
(x, u) 7→ x′ or P (x′ |x, u) 9:23
(x, u) 7→ y or P (y|x, u)
System Id examples: Dynamics
9:20
• If the dynamics ẋ = f (x, u) are unknown, learn them from data!
Regression options for system identification
Literature:
• Linear: (linear in finite number of parameters)
Moore: Acquisition of Dynamic Control Knowledge for a Robotic Manip-
f (x; θ) = φ(x)⊤θ ulator (ICML 1990)
Atkeson, Moore & Schaal: Locally weighted learning for control. Artifi-
cial Intelligence Review, 1997.
• Blackbox parameteric:
Schaal, Atkeson & Vijayakumar: Real-Time Robot Learning with Locally
– Given some blackbox parameteric model f (x; θ) with finite Weighted Statistical Learning. (ICRA 2000)
parameters θ; use blackbox optimization Vijayakumar et al: Statistical learning for humanoid robots, Autonomous
Robots, 2002.
• Non-parameteric:
9:24
– Kernel methods
– Gaussian processes
– Are closely related to linear models
• In all cases one typically minimizes the squared error
Pn
Lls (θ) = i=1 (yi − f (xi ; θ))2
1 ls
• We can use the mean n
L (θ) as estimate of the output variance
2
σ to define
P (y|x; θ) = N(y|f (x; θ), σ 2 )
9:21
(Schaal, Atkeson, Vijayakumar)

System Id examples: Kinematics
• If the kinematics φ are unknown, learn them from data! • Use a simple regression method (locally weighted Linear Re-
gression) to estimate ẋ = f (x, u)
9:25
Literature:
Regression basics
Todorov: Probabilistic inference of multi-joint movements, skeletal pa-
rameters and marker attachments from diverse sensor data. (IEEE
Transactions on Biomedical Engineering 2007) [ML slides]
Deisenroth, Rasmussen & Fox: Learning to Control a Low-Cost Manip-
9:26
ulator using Data-Efficient Reinforcement Learning (RSS 2011)
9:22
Applying System Id to the racer?
• Core problem:
We have no ground truth data!
• We can record data (u, y) (controls & observations), but not x!

Todorov: Probabilistic inference of multi-joint movements, skeletal pa-
rameters and marker attachments from diverse sensor data. (IEEE
Transactions on Biomedical Engineering 2007)
• Try an EM like approach:
– Hand-estimate the parameters as good as possible
– Use a Kalman filter (better: smoother!!) to estimate the

unobserved x during
– Option (a): Learn local linear models q̈ = Ax + a + Bu and
y = Cx + c + Du
Option (b): Improve the parameters θ = (mA , IA , mB , IB , r, l, lC , c1 , .., c5 )
– Repeat with Kalman smoothing
• I have no idea whether/how well this’ll work

9:27
Data
9:28
We’ve collected data
• Motor responses
– Free running wheels (no load..)
– Setting extreme target velocities v ∗ and different accelera-
tion levels a∗ ∈ {−10, .., −1, 1, .., 10} we can generate well-
defined accelerations
• Balancing trials
– the gyroscope picks up some oscillations
– the accelerometer is very noisy, perhaps correlated with
jerky controls
– only 30Hz!
9:29
10 Reinforcement Learning in Robotics a0 a1 a2
s0 s1 s2
r0 r1 r2
QT
P (s0:T +1 , a0:T , r0:T ; π) = P (s0 ) t=0 P (at |st ; π) P (rt |st , at ) P (st+1 |st , at )
RL = Learning to Act
– world’s initial state distribution P (s0 )

– world’s transition probabilities P (st+1 | st , at )
– world’s reward probabilities P (rt | st , at )
– agent’s policy π(at | st ) = P (a0 |s0 ; π) (or deterministic at =
π(st ))
• Stationary MDP:
– We assume P (s′ | s, a) and P (r|s, a) independent of time
R
– We also define R(s, a) := E{r|s, a} = r P (r|s, a) dr
from Satinder Singh’s Introduction to RL, videolectures.com 10:4
10:1
... in the notation this Robotic’s lecture
• We have a (potentially stochastic) controlled system
ẋ = f (x, u) + noise(x, u)
• We have costs (neg-rewards), e.g. in the finite horizon case:

Z T
Jπ = c(x(t), u(t)) dt + φ(x(T ))
(around 2000, by Schaal, Atkeson, Vijayakumar) 0
• We want a policy (“controller”)
π : (x, t) 7→ u
10:5
(2007, Andrew Ng et al.)
10:2
Reinforcement Learning = the dynamics f and costs c are unknown

Applications of RL
• Robotics
– Navigation, walking, juggling, helicopters, grasping, etc... • All the agent can do is collect data
• Games
– Backgammon, Chess, Othello, Tetris, ...
D = {(xt , ut , ct )}Tt=0
• Control
– factory processes, resource control in multimedia networks, elevators,
....
• Operations Research
What can we do with this data?
– Warehousing, transportation, scheduling, ... 10:6
10:3
Five approaches to RL
Markov Decision Process
experience data demonstration data

D = {(xt , ut , ct )}Tt=0 D = {(x0:T , u0:T )d }n 10:10
d=1
learn model learn value fct. optimize policy learn policy learn latent costs
Imitation Learning
P (x′ |u, x) V (x) π(x) π(x) c(x)
c(x, u)
Imitation Learning
dynamic prog. policy dynamic prog.
Vt (x) π(x) Vt
Policy Search
Model−based
policy
Model−free
Inverse OC
policy
πt (x) π(x)
10:7
Five approaches to RL
experience data demonstration data

D = {(st , at , rt )}Tt=0 D = {(s0:T , a0:T )d }n
d=1
Atkeson & Schaal
10:11
learn model learn value fct. optimize policy learn policy learn latent costs
P (s′ |s, a) V (s) π(s) π(s) R(s, a)
R(s, a)
Inverse RL
Imitation Learning
dynamic prog. policy dynamic prog.

V (s) π(s) V (s)
Policy Search
Model−based
policy
Model−free
policy
Inverse RL
learn DP
π(s) π(s) D = {(s0:T , a0:T )d }n
d=1 → R(s, a) → V (s) → π(s)
• Use ML to “uncover” the latent reward function in observed be-

10:8 havior
Imitation Learning
Literature:
learn/copy Pieter Abbeel & Andrew Ng: Apprenticeship learning via inverse rein-
D = {(s0:T , a0:T )d }n
d=1 → π(s) forcement learning (ICML 2004)
Andrew Ng & Stuart Russell: Algorithms for Inverse Reinforcement Learn-
ing (ICML 2000)
• Use ML to imitate demonstrated state trajectories x0:T Nikolay Jetchev & Marc Toussaint: Task Space Retrieval Using Inverse
Feedback Control (ICML 2011).
10:12
Literature:
Atkeson & Schaal: Robot learning from demonstration (ICML 1997)

Inverse RL (Apprenticeship Learning)
Schaal, Ijspeert & Billard: Computational approaches to motor learning
by imitation (Philosophical Transactions of the Royal Society of London. • Given: demonstrations D = {xd0:T }n
d=1
Series B: Biological Sciences 2003)
Grimes, Chalodhorn & Rao: Dynamic Imitation in a Humanoid Robot • Try to find a reward function that discriminates demonstra-
through Nonparametric Probabilistic Inference. (RSS 2006) tions from other policies
Rüdiger Dillmann: Teaching and learning of robot tasks via observation
of human performance (Robotics and Autonomous Systems, 2004) – Assume the reward function is linear in some features R(x) =
10:9 w⊤φ(x)
– Iterate:
Imitation Learning
1. Given a set of candidate policies {π0 , π1 , ..}
• There a many ways to imitate/copy the oberved policy: 2. Find weights w that maximize the value margin between
teacher and all other candidates
Learn a density model P (at | st )P (st ) (e.g., with mixture of Gaus-
max ξ
sians) from the observed data and use it as policy (Billard et al.) w,ξ
s.t. ∀πi : w⊤ hφi ≥ w⊤ hφiπi +ξ

| {z D} | {z }
value of demonstrations value of πi
Or trace observed trajectories by minimizing perturbation costs
2
(Atkeson & Schaal 1997) ||w|| ≤ 1
∂V (β)
3. Compute a new candidate policy πi that optimizes R(x) = • Another is PoWER, which requires ∂β
=0
⊤
w φ(x) and add to candidate list. PH
E{ξ|β} ǫt Qπ (st , at , t)
β←β+ Pt=0
H
(Abbeel & Ng, ICML 2004) E{ξ|β} t=0 Qπ (st , at , t)
10:13
10:17
10:14 Kober & Peters: Policy Search for Motor Primitives in Robotics, NIPS 2008.
(serious reward shaping!)

Policy Search with Policy Gradients
10:18
10:15
Learning to walk in 20 Minutes
Policy gradients
• Policy Gradient method (Reinforcement Learning)
• In continuous state/action case, represent the policy as linear in Stationary policy parameterized as linear in features u =
P
wi φi (q,
i
arbitrary state features:
• Problem: find parameters wi to minimize expected costs
k
X
π(s) = φj (s)βj = φ(s)⊤β (deterministic) cost = mimick (q, q̇) of the passive down-hill walker at “certain
j=1 point in cycle”
⊤
π(a | s) = N(a | φ(s) β, Σ) (stochastic)
with k features φj .
• Given an episode ξ = (st , at , rt )H

t=0 , we want to estimate
Learning To Walk
∂V (β)
∂β Tedrake, Zhang & Seung: Stochastic policy gradient reinforcement learning on a
simple 3D biped. IROS, 2849-2854, 2004. http://groups.csail.mit.edu/
robotics-center/public_papers/Tedrake04a.pdf
10:16
10:19
Policy Gradients
Policy Gradients – references
• One approach is called REINFORCE:
Peters & Schaal (2008): Reinforcement learning of motor skills with policy gradi-
Z Z ents, Neural Networks.
∂V (β) ∂ ∂
= P (ξ|β) R(ξ) dξ = P (ξ|β) log P (ξ|β)R(ξ)dξ Kober & Peters: Policy Search for Motor Primitives in Robotics, NIPS 2008.
∂β ∂β ∂β
H H Vlassis, Toussaint (2009): Learning Model-free Robot Control by a Monte Carlo
∂ X∂ log π(at |st ) X ′
= E{ξ|β} log P (ξ|β)R(ξ) = E{ξ|β} γt γt −t
rt′ EM Algorithm. Autonomous Robots 27, 123-130.
∂β ∂β
t=0 t′ =t
| {z } Rawlik, Toussaint, Vijayakumar(2012): On Stochastic Optimal Control and Rein-
Qπ (st ,at ,t) forcement Learning by Approximate Inference. RSS 2012. (ψ-learning)
• Another is Natural Policy Gradient

– Estimate the Q-function as linear in the basis functions ∂
∂β
log π(a|s): • These methods are sometimes called white-box optimization:
h ∂ log π(a|s) i⊤ They optimize the policy parameters β for the total reward R =
Q(x, u) ≈ w P t
∂β γ rt while tying to exploit knowledge of how the process is
actually parameterized
– Then the natural gradient ( ∂V∂β(β) multiplied with inv. Fisher
10:20
metric) updates
β new = β + αw
Black-Box Optimization Optimizing and Learning
10:21 • Black-Box optimization is strongly related to learning:

• When we have local a gradient or Hessian, we can take that
“Black-Box Optimization” local information and run – no need to keep track of the history
or learn (exception: BFGS)
• The term is not really well defined
• In the black-box case we have no local information directly ac-
– I use it to express that only f (x) can be evaluated cessible
– ∇f (x) or ∇2f (x) are not (directly) accessible → one needs to account for the history in some way or another
More common terms: to have an idea where to continue search
• “Accounting for the history” very often means learning: Learning
• Global optimization a local or global model of f itself, learning which steps have
– This usually emphasizes that methods should not get stuck been successful recently (gradient estimation), or which step
in local optima directions, or other heuristics
– Very very interesting domain – close analogies to (active)
10:25
Machine Learning, bandits, POMDPs, optimal decision mak-
ing/planning, optimal experimental design
Stochastic Search
– Usually mathematically well founded methods
10:26
• Stochastic search or Evolutionary Algorithms or Local Search

Stochastic Search
– Usually these are local methods (extensions trying to be
“more” global)
• The general recipe:
– Various interesting heuristics
– Some of them (implicitly or explicitly) locally approximating – The algorithm maintains a probability distribution pθ (x)
gradients or 2nd order models – In each iteration it takes n samples {xi }n
i=1 ∼ pθ (x)
10:22
– Each xi is evaluated → data {(xi , f (xi ))}n
i=1
– That data is used to update θ

Black-Box Optimization
• Problem: Let x ∈ Rn , f : Rn → R, find • Stochastic Search:
Input: initial parameter θ, function f (x), distribution model

min f (x) pθ (x), update heuristic h(θ, D)
x
Output: final θ and best point x
1: repeat
where we can only evaluate f (x) for any x ∈ Rn Sample {xi }n
2: i=1 ∼ pθ (x)
3: Evaluate samples, D = {(xi , f (xi ))}n
i=1
4: Update θ ← h(θ, D)
5: until θ converges
• A constrained version: Let x ∈ Rn , f : Rn → R, g : Rn → {0, 1}, find
10:27
min f (x) s.t. g(x) = 1
x
Stochastic Search
where we can only evaluate f (x) and g(x) for any x ∈ Rn
I haven’t seen much work on this. Would be interesting to consider this more
rigorously. • The parameter θ is the only “knowledge/information” that is be-
10:23 ing propagated between iterations
θ encodes what has been learned from the history
A zoo of approaches θ defines where to search in the future
• People with many different backgrounds drawn into this

Ranging from heuristics and Evolutionary Algorithms to heavy mathematics • Evolutionary Algorithms: θ is a parent population
Evolution Strategies: θ defines a Gaussian with mean & vari-
– Evolutionary Algorithms, esp. Evolution Strategies, Covari- ance
ance Matrix Adaptation, Estimation of Distribution Algorithms
Estimation of Distribution Algorithms: θ are parameters of
– Simulated Annealing, Hill Climing, Downhill Simplex
some distribution model, e.g. Bayesian Network
– local modelling (gradient/Hessian), global modelling
Simulated Annealing: θ is the “current point” and a temperature
10:24
10:28
Example: Gaussian search distribution (µ, λ)- – Update σ depending on |pσ |

ES – Update C depending on pc p⊤c (rank-1-update) and Var(D′ )
10:31
From 1960s/70s. Rechenberg/Schwefel
• Perhaps the simplest type of distribution model CMA references
θ = (x̂) , pt (x) = N(x|x̂, σ 2 ) Hansen, N. (2006), ”The CMA evolution strategy: a comparing
review”
a n-dimenstional isotropic Gaussian with fixed deviation σ Hansen et al.: Evaluating the CMA Evolution Strategy on Multi-
modal Test Functions, PPSN 2004.
• Update heuristic:
– Given D = {(xi , f (xi ))}λi=1 , select µ best: D′ = bestOfµ (D)
– Compute the new mean x̂ from D′
• This algorithm is called “Evolution Strategy (µ, λ)-ES”

– The Gaussian is meant to represent a “species”
• For “large enough” populations local minima are avoided
– λ offspring are generated
– the best µ selected
10:29 • An interesting variant:
Igel et al.: A Computational Efficient Covariance Matrix Update
Covariance Matrix Adaptation (CMA-ES) and a (1 + 1)-CMA for Evolution Strategies, GECCO 2006.
10:32
• An obvious critique of the simple Evolution Strategies:
– The search distribution N(x|x̂, σ 2 ) is isotropic CMA conclusions
(no going forward, no preferred direction)
• It is a good starting point for an off-the-shelf black-box algorithm
– The variance σ is fixed!
• It includes components like estimating the local gradient (pσ , pC ),
the local “Hessian” (Var(D′ )), smoothing out local minima (large
• Covariance Matrix Adaptation Evolution Strategy (CMA-ES) populations)
10:33
Stochastic search conclusions
Input: initial parameter θ, function f (x), distribution model

pθ (x), update heuristic h(θ, D)
Output: final θ and best point x
1: repeat
2: Sample {xi }ni=1 ∼ pθ (x)
3: Evaluate samples, D = {(xi , f (xi ))}n
i=1
4: Update θ ← h(θ, D)
10:30 5: until θ converges
Covariance Matrix Adaptation (CMA-ES) • The framework is very general

• The crucial difference between algorithms is their choice of pθ (x)
• In Covariance Matrix Adaptation 10:34
θ = (x̂, σ, C, pσ , pC ) , pθ (x) = N(x|x̂, σ 2 C) RL under Partial Observability
where C is the covariance matrix of the search distribution • Data:

• The θ maintains two more pieces of information: pσ and pC cap- D = {(ut , ct , yt )t }Tt=0
ture the “path” (motion) of the mean x̂ in recent iterations → state xt not observable
• Rough outline of the θ-update:
– Let D′ = bestOfµ (D) be the set of selected points • Model-based RL is dauting: Learning P (x′ |u, x) and P (y|u, x)
– Compute the new mean x̂ from D′ with latent x is very hard
– Update pσ and pC proportional to x̂k+1 − x̂k
• Model-free: The policy needs to map the history to a new con-

trol
π : (yt−h,..,t-1 , ut−h,..,t−1 ) 7→ ut
or any features of the history
ut = φ(yt−h,..,t-1 , ut−h,..,t−1 )⊤w
10:35
Features for the racer?
• Potential features might be:

yt , ẏt , hyi0.5 , hẏi0.5 , hyi0.9 , hẏi0.9 , ut , ut−1
yt −yt−1
where ẏ = τt
and hyiα is a low-pass filter
10:36
11 SKIPPED THIS TERM – Grasping (brief 11:3
intro)
Outline
Force closure, alternative/bio-inspired views
• Introduce to the basic classical concepts for grasping (force clo-
Grasping sure)
• The most elementary type of interaction with (manipulation of)

• Discussion and alternative views
the environment.
– Basis for intelligent behavior.
• References:
• In industrial settings with high precision sensors and actuators:
very fast and precise. Craig’s Introduction to robotics: mechanics and control – chapter 3.
Matt Mason’s lecture: Static and Quasistatic Manipulation

• In general real world with uncertain actuators and perception, www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lecture18.
pdf
still a great research challenge, despite all the theory that has
been developed. Daniela Rus and Seth Teller’s lecture: Grasping and Manipulation
11:1 courses.csail.mit.edu/6.141/spring2011/pub/lectures/Lec13-Manipu
pdf
Pick-and-place in industry 11:4
Force Closure
• Which of these objects is in “tight grip”?
(This type of kinematics is called “Delta Robot”, which is a “par-

allel robot” just as the Stewart platform.) Defining “tight grip”: Assume fingers (vectors) have no friction
11:2 – but can exert arbitrary normal forces. Can we generate (or
counter-act) arbitrary forces on the object?
Research 11:5
• Using ultra high speed and precise cameras and localization:

Force Closure – more rigorously
High speed robot hand from the Ishikawa Komuro’s “Sensor Fu-
sion” Lab • Assume that each “finger” is a point that can apply forces on the
object as decribed by the friction cone
http://www.k2.t.u-tokyo.ac.jp/fusion/index-e.html
• Asimo’s grasping: • Each finger is described by a point pi and a force fi ∈ Fi in the

fingers friction cone. Together they can exert the the force an
torque:
X X
f total = fi , τ total = fi × (pi − c)
i i
• Force closure ⇐⇒ we can generate (counter-act) arbitrary 11:9

total total
f and τ by choosing fi ∈ Fi appropriately.
The “mitten thought experiment”
↔ Check whether the positive linear span of the fiction cones
covers the whole space. • From Oliver Brock’s research website:
11:6 “Our approach to grasping is motivated by the ”mitten thought experi-
ment”. This experiment illustrates that a sensory information-deprived
subject (blindfolded, wearing a thick mitten to eliminate tactile feedback)
Form & Force Closure is able to grasp a large variety of objects reliably by simply closing the
hand, provided that a second experimenter appropriately positioned the
object relative to the hand. This thought experiments illustrates that an
• Force closure: The contacts can apply an arbitrary wrench (=force- appropriate perceptual strategy (the experimenter) in conjunction with a
torque) to the object. simple compliance-based control strategy (the mitten hand) can lead to
outstanding grasping performance.”
• Form closure: The object is at an isolated point in configuration

space. Note: form closure ⇐⇒ frictionless force closure
• Equilibrium: The contact forces can balance the object’s weight

Illustration from O. Brock’s page
and other external forces.
11:10
11:7
Food for thought

Traditional research into force closure
• Are point contact a good model?
• Theorem (Mishra, Schwartz and Sharir, 1987): • Is the whole idea of “arranging friction cones” the right approach?
For any bounded shape that is not a surface of revolution, a • What about biomechanics?
force closure (or first order form closure) grasp exists. 11:11
• Guaranteed synthesis:
Biomechanics of the human hand
1) put fingers “everywhere” • Finger tendons:
2) while redundant finger exists delete any redundant finger
(A finger is redundant if it can be deleted without reducing the
positive linear span.)
Theorem (Mishra, Schwartz, and Sharir, 1987):

For any surface not a surface of revolution, [the above method] J.N.A.L. Leijnse, 2005
yields a grasp with at most 6 fingers in the plane, at most 12 De Bruijne et al. 1999
fingers in three space.
11:8
See Just Herder’s lecture on “Biograsping”
http://www.slideshare.net/DelftOpenEr/bio-inspired-design-lectu
Traditional research into force closure 11:12
• Force closure turn into a continuous optimization criterion: Tendon-based hand mechanisms
– Constrain the absolute forces each finger can apply

(cut the friction cones)
– The friction cones define a finite convex polygon in 6D wrench
space
Shape Gripper, Shigeo Hirose, TITECH Jaster Schuurmans, 2004
→ What is the inner radius of this convex wrench polygon?
Illustration from Suárez, Roa, Cornellà (2006): Grasp Quality Measures

DLR hand
11:13
12 SKIPPED THIS TERM – Legged Lo-

comotion (brief intro)
• Three separate controllers for:
– hopping height
Why legs, Raibert hopper, statically stable walking, zero moment
– horizontal velocity (foot place-
point, human walking, compass gait, passive walker ment)
– attitude (hip torques during
stance)
Legged Locomotion
• Each a simple (PD-like) controller
• Why legs?
Raibert et al.: Dynamically Stable Legged Locomotion. 1985 http://dspace.

Bacterial Flagellum: (rotational “motors” in Biology?) mit.edu/handle/1721.1/6820
Tedrake: Applied Optimal Control for Dynamically Stable Legged Locomotion.

PhD thesis (2004). http://groups.csail.mit.edu/robotics-center/
public_papers/Tedrake04b.pdf
12:4
Biped locomotion
• Walking vs Running
12:1 Walking := in all instances at least one foot is on ground
Running := otherwise
Legged Locomotion
• 2 phases of Walking
• Why legs?
– double-support phase (in Robotics often statically stable)
– single-support phase (statically instable)
– Human/Animal Locomotion Research
– Potentially less weight
– Better handling of rough terrains

(climbing, isolated footholds, ladders, stairs)
12:5
12:2
Asimo
Rolling vs walking
12:6
Statically Stable Walk
• You could rest (hold pose) at any point in time and not fall over
⇐⇒ CoG projected on ground is within support polygon
CoG = center of gravity of all body masses

12:3 support polygon = hull of foot contact points
One-legged locomotion • Try yourself: Move as slow as you can but make normal length
steps...
12:7
See also: Popovic, Goswami & Herr: Ground Reference Points in Legged Loco-
motion: Definitions, Biological Trajectories and Control Implications. International
Journal of Robotics Research 24(12), 2005. http://www.cs.cmu.edu/˜cga/
Zero Moment Point legs/Popovic_Goswami_Herr_IJRR_Dec_2005.pdf
12:10
• Vukobratovic’s view on walking, leading to ZMP ideas:
“Basic characteristics of all biped locomotion systems are: Zero Moment Point
• If the ZMP is outside the support polygon → foot rolls over the
(i) the possibility of rotation of the overall system about one of
edge
the foot edges caused by strong disturbances, which is equiva-
lent to the appearance of an unpowered (passive) DOF, (presumes foot is a rigid body, and non-compliant control)
(ii) gait repeatability (symmetry), which is related to regular gait • Locomotion is dynamically stable if the ZMP remains within the
only foot-print polygons.
(↔ the support can always apply some momentum, if neces-
sary)
(iii) regular interchangeability of single- and double-support phases”
• Computing the ZMP in practise:

Vukobratovic & Borovac: Zero-moment point—Thirty five years of its life. Interna- – either exact robot model
tional Journal of Humanoid Robotics 1, 157-173, 2004. http://www.cs.cmu.
edu/˜cga/legs/vukobratovic.pdf
– or foot pressure sensors
12:8
12:11
Zero Moment Point Zero Moment Point – example
• combine ZMP with 3D linear inverted pendulum model and model-

predictive control
HRP-2 stair climbing
Kajita et al.: Biped Walking Pattern Generation by using Preview Control of Zero-
Moment Point. ICRA 2003. http://eref.uqu.edu.sa/files/eref2/folder1/
biped_walking_pattern_generation_by_usin_53925.pdf
12:9 12:12
Zero Moment Point ZMP Summary
• “ZMP is defined as that point on the ground at which the net mo- • ZMP is the “rescue” for conventional methods:
ment of the inertial forces and the gravity forces has no compo-
– ZMP-stability → the robot can be controlled as if foot is “glued”
nent along the horizontal axes.” (Vukobratovic & Borovac, 2004)
(virtually) to the ground!
– The whole body behaves like a “conventional arm”
• fi = force vector acting on body i (gravity plus external)
– Can accellerate q̈ any DoF → conventional dynamic control
wi = angular vel vector of body i fully actuated system
3×3
Ii = interia tensor (∈ R ) of body i
ri = pi − pZMP = relative position of body i to ZMP • Limitations:
– Humans don’t use ZMP stability, we allow our feet to roll
• Definition: pZMP is the point on the ground for which (toe-off, heel-strike: ZMP at edge of support polygon)
P ! ⊤
i ri × fi + Ii ẇi + wi × Ii wi = (0, 0, *) – Can’t describe robots with point feet (walking on stilts)
12:13
Models of human bipedal locomotion
The following illustrations are from:
McMahon: Mechanics of Locomotion. IJRR 3:4-28, 1984

http://www.cs.cmu.edu/˜cga/legs/mcmahon1.pdf
http://www.cs.cmu.edu/˜cga/legs/mcmahon2.pdf
12:14 4. Stance Knee Flexion:
Walking research from Marey 1874:
5. Stance Ankle Flextion:
12:15
6. Pelvis Lateral Displacement:

Six determinants of gait
following Saunders, Inman & Eberhart (1953)
1. Compass Gait:
12:16
2. Pelvic Rotation:
Models of human bipedal locomotion
• Human model with 23 DoFs, 54 muscles

– Compare human walking data with model
– Model: optimize energy-per-distance
– Energy estimated based on metabolism and muscle heat rate
models
3. Pelvic Tilt:
Passive walker examples
compass gait simulation
controlled on a circle
passive walker
Anderson & Pandy: Dynamic Optimization of Human Walking. Journal of Biome-

chanical Engineering 123:381-390, 2001. http://e.guigon.free.fr/rsc/ • Minimally actuated: Minimal Control on rough terrain
article/AndersonPandy01.pdf
12:21
Anderson & Pandy: Static and dynamic optimization solutions for gait are prac-
tically equivalent. Journal of Biomechanics 34 (2001) 153-161. http://www.
bme.utexas.edu/faculty/pandy/StaticOptWalking2001.pdf
12:17 Impact Models in the Compass Gait
Models of human bipedal locomotion • Switch between two consecutive swing phases: depends on
slope!
• Suggest different principles of human motion:
• Typical assumptions made in simulation models:
– passive dynamics (Compass Gait) ↔ underactuated system – The contact of the swing leg with the ground results in no rebound and
no slipping of the swing leg.
– modulation of basic passive dynamics
– At the moment of impact, the stance leg lifts from the ground without
– Energy minimization interaction.
– The impact is instantaneous.
12:18
– The external forces during the impact can be represented by impulses.
– The impulsive forces may result in an instantaneous change in the
Passive dynamic walking: Compass Gait velocities, but there is no instantaneous change in the configuration.
– The actuators cannot generate impulses and, hence, can be ignored

• Basic 2D planar model of the Compass Gait:
during impact.
Westervelt, Grizzle & Koditschek: Hybrid Zero Dynamics of Planar Biped Walkers.
IEEE Trans. on Automatic Control 48(1), 2003.
http://repository.upenn.edu/cgi/viewcontent.cgi?article=1124&con
ese_papers
12:22
Implausibility of the stiff Compass Gait leg
The pose is described by q = (θs , θns ), the state by (q, q̇)
Goswami, Thuilot & Espiau: A study of the passive gait of a compass-like biped
robot: symmetry and chaos. International Journal of Robotics Research 17, 1998.
http://www.ambarish.com/paper/COMPASS_IJRR_Goswami.pdf
12:19
Passive dynamic walking: Compass Gait
• Swing phase has analytic equations of motions

M (q)q̈ + C(q, q̇)q̇ + G(q) = 0
but can’t be solved analytically...
• Phase space plot of numeric solution: Geyer, Seyfarth & Blickhan: Compliant leg behaviour explains basic dynamics of
walking and running. Proc. Roy. Soc. Lond. B, 273(1603): 2861-2867, 2006.
http://www.cs.cmu.edu/˜cga/legs/GeyerEA06RoySocBiolSci.pdf
12:23
Learning to walk in 20 Minutes
• Policy Gradient method (Reinforcement Learning)

P
Stationary policy parameterized as linear in features u = i wi φi (q,
• Problem: find parameters wi to minimize expected costs
12:20
cost = mimick (q, q̇) of the passive down-hill walker at “certain
point in cycle”
12:27
Learning To Walk
Tedrake, Zhang & Seung: Stochastic policy gradient reinforcement learning on a

simple 3D biped. IROS, 2849-2854, 2004. http://groups.csail.mit.edu/
robotics-center/public_papers/Tedrake04a.pdf
12:24
Summary
• ZMP type walking was successful (ASIMO, HRP-2, etc), but lim-
ited
• Future types of walking:

– Exploit passive dynamics, cope with underactuation
– Follow some general optimiality principle (but real-time!)
– Learn (esp. Reinforcement Learning)
– Compliant hardware! (controllable elasticity & damping)
• Recommended reading: Tedrake: Underactuated Robotics: Learn-

ing, Planning, and Control for Effcient and Agile Machines. Course
Notes for MIT 6.832
www.cs.berkeley.edu/˜pabbeel/cs287-fa09/readings/Tedrake-Aug09.
pdf
12:25
Finally, multi-legged locomotion
12:26
Finally, multi-legged locomotion
http://www.bostondynamics.com/robot_rise.html
13 Exercises liblapack-dev freeglut3-dev libqhull-dev libf2c2-dev

libann-dev gnuplot doxygen
13.1 Exercise 1 • get the code from

http://ipvs.informatik.uni-stuttgart.de/mlr/marc/
source-code/libRoboticsCourse.13.tgz
13.1.1 Geometry
• tar xvzf libRoboticsCourse.13.tgz
cd share/examples/Ors/ors
make
Read the notes on basic 3D geometry at http://ipvs. ./x.exe
informatik.uni-stuttgart.de/mlr/marc/notes/3d-geometry.pdf
at least until section 2. We will recap this briefly also in
the lecture. Prepare questions for the exercises if you
have any.
a) You have a book (coordinate frame B) lying on the 13.2 Exercise 2

table (world frame W ). You move the book 1 unit to the
right, then rotate it by 45◦ counter-clock-wise. Given
13.2.1 Task spaces and Jacobians
a dot p marked on the book at position pB = (1, 1) in
the book coordinate frame, what are the coordinates
pW of that dot with respect to the world frame? Given In the lecture we introduced the basic kinematic maps
a point x with coordinates xW = (0, 1) in world frame, φpos vec pos
eff,v (q) and φeff,v (q), and their Jacobians, Jeff,v (q) and
vec
what are its coordinates xB in the book frame? What is Jeff,v (q). In the following you may assume that we know
the coordinate transformation from world frame to book how to compute these for any q. The problem is to
frame, and from book frame to world frame? express other kinematic maps and their Jacobians in
terms of these knowns.
a) Assume you would like to control the pointing direc-

13.1.2 Vector derivatives tion of the robot’s head (e.g., its eyes) to point to an
external world point xW . What task map can you de-
Let x ∈ Rn , y ∈ Rd , f, g : Rn → Rd , A ∈ Rd×n , C ∈ fine to achieve this? What is the Jacobian?
Rd×d .
b) You would like the two hands or the robot to become
∂ parallel (e.g. for clapping). What task map can you de-
a) What is ∂x x ?
fine to achieve this? What is the Jacobian?
∂ ⊤
b) What is ∂x [x x] ?
c) You would like to control a standard endeffector po-
c) What is ∂ ⊤
∂x [f (x) f (x)] ? sition peff to be at y ∗ , as usual. Can you define a 1-
dimensional task map φ : Rn → R to achieve this?
∂ ⊤
d) What is ∂x [f (x) Cg(x)] ? What is its Jacobian?
e) Let B and C be symmetric (and pos.def.). What is

the minimum of (Ax − y)⊤C(Ax − y) + x⊤Bx ?
13.2.2 IK in the simulator
13.1.3 Simulation software Download the simulator code from http://userpage.fu-berlin.

de/˜mtoussai/source-code/libRoboticsCourse.13.tgz. (See last
exercise for instructions.) The header <src/Ors/roboticsCou

Future exercises will require to code some examples
provides a very simple interface to the simulator—we
in C/C++. Test if you can compile and run the lib that
will use only this header and some generic matrix func-
accompanies this lecture. Report on problems with in-
tionalities.
stallation.
Consider the example in teaching/RoboticsCourse/01-kin
On Ubuntu:
(rename main.problem.cpp to main.cpp). The goal
is to reach the coordinates y ∗ = (−0.2, −0.4, 1.1) with
• install the packages the right hand of the robot. Assume W = I and σ = .01.
a) The example solution generates a motion in one step Bonus: How can we apply joint space interpolation?
using inverse kinematics δq = J ♯ δy with J ♯ = (J⊤J + How could one avoid zero velocities at the junction of
σ 2 W )-1 J⊤. Record the task error, that is, the deviation sections?
of hand position from y ∗ after the step. Why is it larger
than expected?
b) Try to do 100 smaller steps δq = αJ ♯ δy with α = .1

13.4 Exercise 4
(each step starting with the outcome of the previous
step). How does the task error evolve over time?
13.4.1 The Dijkstra algorithm
c) Generate a nice trajectory composed of T = 100 time
steps. Interpolate the target linearly ŷ ← y0 +(t/T )(y ∗ −
Write a proper pseudo code for the Dijkstra algorithm
y0 ) in each time step.
on a general undirected graph G = (V, E). A graph is
d) Generate a trajectory that moves the right hand in defined by the set V of nodes and the set E of edges;
a circle centered at (−0.2, −0.4, 1.1), aligned with the each edge e ∈ E is a tuple e = (v1 , v2 ) of nodes.1 De-
xz-plane, with radius 0.2. termine the computational complexity of the algorithm.
13.4.2 RRTs for path finding

13.3 Exercise 3
In our libRoboticsCourse.12.tgz in teaching/Robotics
you find an example problem (rename main.problem.cpp
In the tutorial we will first discuss again the last week’s
to main.cpp).
2nd exercise.
a) The code demonstrates an RRT exploration and dis-
plays the explored endeffector positions. What is the
13.3.1 Motion profiles endeffector’s exploration distribution in the limit n →
∞? Specify such a distribution analytically for a planar
2 link arm.
Construct a motion profile that accelerates constantly
in the first quarter of the trajectory, then moves with b) First grow an RRT backward target configuration q ∗ =
constant velocity, then decelerates constantly in the last (0.945499, 0.431195, −1.97155, 0.623969, 2.22355, −0.665206,
quarter. Write down the equation MP : [0, 1] 7→ [0, 1]. −1.48356) that we computed in the last exercises. Stop
when there exists a node close (<stepSize) to the
q = 0 configuration. Read out the collision free path
from the tree and display it. Why would it be more diffi-
13.3.2 Multiple task variables & Peg in a Hole
cult to grow the tree forward from q = 0 to q ∗ ?
In our libRoboticsCourse.12.tgz in teaching/RoboticsCourse/02-pegInAHole

c) Find a collision free path using bi-directional RRTs
you find an example problem (rename main.problem.cpp (that is, 2 RRTs growing together). Use q ∗ to root the
to main.cpp), where the goal is to stick the green peg backward tree and q = 0 to root the forward tree. Stop
into the blue “hole”. when a newly added node is close to the other tree.
Read out the collision free path from the tree and dis-
The initial implementation fails: it does not find an ap- play it.
propriate path to insert the peg from the top; and it
does not use kinematicsVec(y,"peg") with target d) (Bonus) Think of a method to make the found path
ARR(0,0,-1) to force the peg to point down. smoother (while keeping it collision free). You’re free
to try anything. Implement the method and display the
Implement a nice peg-in-a-hole movement. You may smooth trajectory.
devide the whole movement in several sections and use
1
task space or joint space interpolations with a smooth Ideally, use the LaTeX package algpseudocode to
motion profiles within each. The motion should reach write the pseudo code (see http://en.wikibooks.org/
the final position with very high accuracy in finite time wiki/LaTeX/Algorithms#Typesetting_using_the_
and without collisions. algorithmicx_package )
e) (Bonus) Follow the smooth trajectory using a sinus of the subroutines needed in lines 4-6 of the algorithm
motion profile using kinematic control. on slide 03:58. (No need to implement it.)
Consider a current state x0 = (0, 1) (at position 0 with

velocity 1). Pick any random phase state xtarget ∈ R2 .
How would you connect x0 with xtarget in a way that ful-
13.5 Exercise 5
fils the differential constraints of the point mass dynam-
ics? Given this trajectory connecting x0 with xtarget , how
In the lecture we discussed PD force control on a 1D would you quantify/meassure the distance? (If you de-
point mass, which leads to oscillatory behavior for high fined the connecting trajectory appropriately, you should
Kp and damped behavior for high Kd (slide 05:13). Slide be able to give an analytic expression for this distance.)
05:14 replaces the parameters Kp , Kd by two other, Given a set (tree) of states x1:n and you pick the clos-
more intuitive parameters, λ and ξ: λ roughly denotes est to xtarget , how would you “grow” the tree from this
the time (or time steps) until the goal is reached, and closest point towards xtarget ?
ξ whether it is reached agressively (ξ > 1, which over-
shoots a bit) or by exponential decay (ξ ≤ 1). Use this
to solve the following exercise.
13.6 Exercise 6
13.5.1 PD force control on a 1D mass point

13.6.1 Direct PD control to hold an arm steady
a) Implement the system equation for a 1D point mass
with mass m = 3.456. That is, implement the Euler in- In our code, in 03-dynamics you find an example (re-
tergration of the system dynamics that computes xt+1 name main.problem.cpp to main.cpp). Please change
given xt and ut in each iteration. (No need for the ../02-pegInAHole/pegInAHole.ors to pegArm.ors.
robot simulator—implement it directly.) Assume a step You will find an arm with three joints that is swinging
time of τ = 0.01sec. Generate a trajectory from the freely under gravity.
start position q0 = 0 that approaches the goal position
a) Apply direct PD control (without using M and F ) to
q ∗ = 1 with high precision within about 1 second us-
each joint separately and try to find parameters Kp and
ing PD force control. Find 3 different parameter sets for
Kd (potentially different for each joint) to hold the arm
Kp and Kd to get oscillatory, overdamped and critical
steady, i.e., q ∗ = 0 and q̇ ∗ = 0. If you are successful,
damped behaviors. Plot the point trajectory (e.g. using
try the same for the arm in pegArm2.ors.
the routine gnuplot(arr& q); MT::wait(); .)
b) (Bonus) Try to use a PID controller that also includes
b) Repeat for time horizon t = 2sec and t = 5sec. How
the integral error
should the values of Kp and Kd change when we have
more time? Z t
∗ ∗
u = Kp (q − q) + Kd (q̇ − q̇) + Ki (q ∗ − q(s)) ds .
s=0
c) Implement a PID controller (including the integral (sta-
tionary error) term). How does the solution behave with
only Ki turned on (Kp = Kd = 0); how with Ki and Kd 13.6.2 PD acceleration control to hold an arm
non-zero?
steady
13.5.2 A distance measure in phase space for As above, try to hold the arm steady at q ∗ = 0 and
q̇ ∗ = 0. But now use the knowledge of M and F in
kinodynamic RRTs each time step. For this, decide on a desired wave-
length λ and damping behavior ξ and compute the re-
Consider the 1D point mass with mass m = 1 state spective Kp and Kd (assuming m = 1), the same for
x = (q, q̇). The 2D space of (q, q̇) combining position each joint. Use the PD equation to determine desired
and velocity is also called phase space. accelerations q̈ ∗ (slide 05:31) and use inverse dynam-
ics to determine the necessary u.
Draft an RRT algorithm for rapidly exploring the phase
space of the point mass. Provide explicit descriptions Try this for both, pegArm.ors and pegArm2.ors.
13.6.3 The dynamic peg-in-a-hole problem 13.7.2 Gaussians
In the exercise 3 you generated nice collision-free tra- On slide 06:11 there is the definition of a multivariate
jectories for peg-in-a-hole using inverse kinematics. (n-dim) Gaussian distribution. Proof the following using
only the definition. (You may ignore terms independent
a) Follow these reference trajectories using PD accel- of x.)
eration control (slide 05:31) and thereby solve the peg-
in-a-hole problem with a noisy dynamic system. a) Proof that:
b) Increase noise into the dynamic system (change to N(x|a, A) = N(a|x, A)

setDynamicSimulationNoise(2.);). Record the
trajectory of the 3rd joint (q(2)) and plot it. Tune the N(x | a, A) = |F | N(F x | F a, F AF⊤)
PD parameters to get an oscillatory behavior. 1
N(F x + f | a, A) = |F | N(x | F -1 (a − f ), F -1 AF -⊤ )
b) Prove:

N(x | a, A) N(x | b, B) ∝ N x (A-1 + B -1 )-1 [A-1 a +
13.7 Exercise 7 B -1 b] , (A-1 + B -1 )-1

c) Prove:
13.7.1 Particle Filtering the location of a car
R
y
N(x | a + F y, A) N(y | b, B) dy = N(x | a + F b, A +
Start from the code in RoboticsCourse/05-car. F BF⊤)
The CarSimulator simulates a car exactly as described

on slide 03:48 (using Euler integration with step size
1sec). At each time step a control signal u = (v, φ)
13.8 Exercise 8
moves the car a bit and Gaussian noise with standard
deviation σdynamics = .03 is added to x, y and θ. Then, in
each step, the car meassures the relative positions to 13.8.1 Kalman filter
some landmars, resulting in an observation yt ∈ Rm×2 ;
these observations are Gaussian-noisy with standard
We consider the same car example as for the last exer-
deviation σobservation = .5. In the current implementation
cise, but track the car using a Kalman filter.
the control signal ut = (.1, .2) is fixed (roughly driving
circles). a) To apply a Kalman filter (slide 07:28) we need Gaus-
sian models for P (xt | xt-1 , ut-1 ) as well as P (yt |xt ). We
a) Odometry (dead reckoning): First write a particle fil-
assume that the dynamics model is given as a local
ter (with N = 100 particles) that ignores the observa-
Gaussian of the form
tions. For this you need to use the cars system dy-
namics (described on 03:48) to propagate each parti-
P (xt+1 | xt , ut ) = N(xt+1 |xt + B(xt )ut , σdynamics )
cle, and add some noise σdynamics to each particle (step
3 on slide 07:23). Draw the particles (their x, y com- where the matrix B(xt ) gives the local linearization of
ponent) into the display. Expected is that the particle the car dynamics (slide 05:27). What is B(xt ) (the Ja-
cloud becomes larger and larger. cobian of the state change w.r.t. u) for the car dynam-
ics?
b) Next implement the likelihood weights wi ∝ P (yt |xit ) =
1 i 2 2
N(yt |y(xit ), σ) ∝ e− 2 (yt −y(xt )) /σ where y(xit ) is the b) Concerning the observation likelihood P (yt |xt ) we
(ideal) observation the car would have if it were in the assume
P
particle possition xit . Since i wi = 1, normalize the
weights after this computation. P (yt |xt , θ1:N ) = N(yt | C(xt )xt + c(xt ), σobservation )
c) Test the full particle filter including the likelihood weights What is the matrix C(xt ) (the Jacobian of the landmark
(step 4) and resampling (step 2). Test using a larger positions w.r.t. the car state) in our example?
(10σobservation ) and smaller (σobservation /10) variance in the
computation of the likelihood. c) Start with the code in RoboticsCourse/06-kalmanSLAM.
Write a Kalman filter to track the car. You can use the 13.9.2 Cart pole swing-up
routine getObservationJacobianAtState to access
∂y θ
C(xt ) = ∂x . Note that c(xt ) = ŷt − C(xt )xt , where ŷt
is the mean observation in state xt (there is another
routine for this). u
13.8.2 Kalman SLAM The cart pole (as described, e.g., in the Sutton-Barto
book) is a standard benchmark to test stable control
strategies. We will assume the model known.
Slide 07:38 outlines how to use a high-dimensional Kalman
filter to simultaneously estimate the robot position (lo- The state of the cart-pole is given by x = (x, ẋ, θ, θ̇),
calization) and the landmarks position (mapping). with x ∈ R the position of the cart, θ ∈ R the pendulums
angular deviation from the upright position and ẋ, θ̇ their
a) Concerning P (yt |xt , θ1:N ) we assume respective temporal derivatives. The only control signal
u ∈ R is the force applied on the cart. The analytic
P (yt |xt , θ1:N ) = N(yt | D(xt )θ + d(xt ), σobservation ) model of the cart pole is
h i
where D(xt ) = ∂θ∂y
is the observation Jacobian w.r.t. the g sin(θ) + cos(θ) −c1 u − c2 θ̇2 sin(θ)
θ̈ = 4 (2)
unknown landmarks, and θ ∈ R2N is the same as θ1:N 2
3 l − c2 cos (θ)
h i
written as a 2N -dim vector. ẍ = c1 u + c2 θ̇2 sin(θ) − θ̈ cos(θ) (3)
Write a pseudo-code for Kalman SLAM. (There is much
with g = 9.8ms2 the gravitational constant, l = 1m the
freedom in how to organize the code, choices of nota-
pendulum length and constants c1 = (Mp + Mc )−1 and
tion and variables, etc. Try to write is as concise as
c2 = lMp (Mp + Mc )−1 where Mp = Mc = 1kg are the
possible.)
pendulum and cart masses respectively.
b) Try to implement Kalman SLAM, which tracks the car
a) Implement the system dynamics using the Euler in-
simultaneous to the landmarks. You should now access
tegration with a time step of ∆ = 1/60s. Test the imple-
the routines getMeanObservationAtStateAndLandmarks
mentation by initializing the pole almost upright (θ = .1)
and getObservationJacobianAtStateAndLandmarks
and watching the dynamics. To display the system,
to retrieve the mean observation and the necessary Ja-
start from the code in course/07-cartPole. The
cobians given the current mean estimate θ of the land-
state of the cart pole can be displayed using OpenGL
marks.
with the state.gl.update() function.
b) Design a controller that stabilizes the pole in upright

position and the cart in the zero position – any heuristic
is allowed (we will use Ricatti methods later). You may
13.9 Exercise 9 want to assume that the range of θ and θ̇ are limited to
some small interval around zero (theoretically the im-
plication is that the local linearization of the system is a
13.9.1 Aggressive quadcopter maneuvers good approximation). Your controller then needs to en-
sure that the system does not escape such an interval.
(It would be rather hard to design a general controller
In the article D. Mellinger, N. Michael and V. Kumar
that can handle any initial state and return the system
(2010): Trajectory generation and control for precise
stably to the target state.)
aggressive maneuvers with quadrotors www.seas.upenn.
edu/˜dmel/mellingerISER2010.pdf the methods used for ag-
Test your controller on two problems:
gressive quadcopter maneuvers are described (see also
the videos at http://www.youtube.com/watch?v=geqip_0Vjec). – When the dynamics are deterministic (as above) but
the initial position is perturbed by θ = .1.
Read the essential parts of the paper to be able to ex-
plain how the quadcopter is controlled. (Neglect the – When additionally the dynamics are stochastic (add
part on parameter adaptation.) Gaussian noise with standard deviation σ = .01 to the
system state in each Euler integration step). respective temporal derivatives. The only control signal
u ∈ R is the force applied on the cart. The analytic
model of the cart pole is
h i
g sin(θ) + cos(θ) −c1 u − c2 θ̇2 sin(θ)
13.10 Exercise 10 θ̈ = (4)
4
3l − c2 cos2 (θ)
h i
p̈ = c1 u + c2 θ̇2 sin(θ) − θ̈ cos(θ) (5)
13.10.1 Read the other exercise
with g = 9.8ms2 the gravitational constant, l = 1m the
On the webpage there is a 2nd exercise sheet e10- pendulum length and constants c1 = (Mp + Mc )−1 and
riccati. Please read this carefully. You don’t need to c2 = lMp (Mp + Mc )−1 where Mp = Mc = 1kg are the
do the exercise – the Octave solution was anyway in pendulum and cart masses respectively.
07-cartPole/cartPole.m. But you need to under-
stand what’s happening – we will do exactly the same a) Derive the local linearization of these dynamics around
for the Racer below. x∗ = (0, 0, 0, 0). The eventual dynamics should be in
the form
ẋ = Ax + Bu
13.10.2 Balance the Racer
Note that
Download the new libRoboticsCourse.13.tgz from    
 0 1 0 0  0
the webpage. This now includes a folder 09-racer, 
 ∂ p̈
 ∂ p̈ ∂ p̈ ∂ p̈ 





 ∂ p̈ 
   
 ∂p ∂ ṗ ∂θ  ∂u 
which simulates the racer using Runge Kutta. Note that A=  ∂ θ̇ 
 , B= 





 0 0 0 1  


0 


q = (x, θ). Currently it applies a control signal u = 0. 
 ∂ θ̈ ∂ θ̈ ∂ θ̈

∂ θ̈   
∂ θ̈
∂p ∂ ṗ ∂θ ∂ θ̇ ∂u
Design a controller π : (q, q̇) 7→ u that balances the
robot. where all partial derivatives are taken at the point p =
ṗ = θ = θ̇ = 0.
The solution (to continue with the other parts) is

13.10.3 Use the local linearization and Alge-
   
braic Riccati equation 0 

1 0 0 

0 

   
0 
 0 0 0 
 c1 

A= 
  , B= 



0 


0 0 1 


0 


The code implements a routine getDynamics that, for 0


0 g
0
 
 −c1 

4 4
3 l−c2 3 l−c2
the current state (q, q̇), computes the local linear dy-
namics
M q̈ + F = Bu b) We assume a stationary infinite-horizon cost function
of the form
Use this to apply the Algebraic Riccati equation, as in
Z ∞
the exercise e10-riccati, to compute a linear regulator π
J = c(x(t), u(t)) dt
using Octave. Test robustness w.r.t. system noise, that 0
is, increasing dynamicsNoise. c(x, u) = x⊤Qx + u⊤Ru
Q = diag(c, 0, 1, 0) , R=I.
That is, we penalize position offset c||p||2 and pole angle

13.11 Exercise 10 offset ||θ||2 . Choose c = ̺ = 1 to start with.
Solve the Algebraic Riccati equation

13.11.1 Local linearization and Algebraic Ric-
0 = A⊤P + P⊤A − P BR-1 B⊤P + Q
cati equation
by initializing P = Q and iterating using the following
The state of the cart-pole is given by x = (p, ṗ, θ, θ̇), iteration:
with p ∈ R the position of the cart, θ ∈ R the pendulums
angular deviation from the upright position and ṗ, θ̇ their Pk+1 = Pk + ǫ[A⊤Pk + Pk⊤A − Pk BR-1 B⊤Pk + Q]
Choose ǫ = 1/1000 and iterate until convergence. Out-

put the gains K = −R-1 B⊤P . (Why should this iteration
converge to the solution of the ARE?) This dynamics ẋ = Ax + a, with x = (q, q̇) simply
says that the velocities are “copied” with high pre-
c) Solve the same Algebraic Riccati equation by calling cision to the next time slice, but the accelerations
the are routine of the octave control package (or a sim- in the next time slice are N(0, 1) distributed (very
ilar method in Matlab). For Octave, install the Ubuntu uncertain). Clearly this is a rough approximation
packages octave3.2, octave-control, and qtoctave, – but fully sufficient for the current scenario.
perhaps use pkg load control and help are in
• In the
octave to ensure everything is installed, use P=are(A,B*inverse(R) Kalman filter loop step as follows:
*B’,Q)
to solve the ARE. Output K = −R-1 B⊤P and compare – Retrieve the observation model (C, c, W ) for the
to b). current Racer state using Racer::getObservation.
Also retrieve ypred here: the predicted sensor read-
(I found the solution K = (1.00000, 2.58375, 52.36463, 15.25927.) ings.
– Use the true sensor readings (from the data file)
d) Implement the optimal Linear Quadratic Regulator and the dynamics and observation model for a
Kalman step. Compute τ from the data files.
u∗ = −R-1 B⊤P x on the cart pole. Increase ̺ (e.g. to
– Set the state of the Racer model to the Kalman
100) and observe how the control strategy changes.
estimate using R.q = ... and R.q_dot = ...,
and display the state using R.gl.update()
– Output the Kalman’s mean estimate x, the pre-
dicted ypred , and the true sensor readings ytrue in
one line of a file
13.12 Exercise 11
• Plot all curves of the output file. In particular,
13.12.1 Kalman filtering compare the predicted sensor outputs ypred with
the true ones ytrue . Do they match?
I collected new data from the racer’s IMU, with higher

frame rate. To collect this data I fixed the wheels (mo-
tors don’t turn, the motors’ encoder is constantly zero) 13.12.2 Identification of the sensor model
and moved the racer by hand back and forth from lying
on the ground to approximately balancing. Now that we have an estimated underlying state tra-
jectory q(t), q̇(t), we can learn an even better sensor
Please find the data files 01-imu.dat, 02-imu.dat,
model. Usually this means to learn a mapping from the
01-times.dat, 02-times.dat on the course page.
dynamic state to the sensor readings:
01 and 02 refer to two different trials—start with 02.
The imu files contain the 4D IMU signal (the 4th entry
is constantly zero: the motor encoders). The times
x(t) 7→ y(t)
files contain the real time in seconds that correspond
to these readings (you will need these to determine the
time interval τ between two steps). However, we exploit that we already have a sensor model
implemented, with hand-tuned parameters, and want to
Also, find on the webpage the two files racer.h and learn a model that improves upon this (or corrects this).
racer.cpp, which implement an updated model of the Therefore we learn a mapping
racer.
Implement a Kalman filter to estimate the state trajec- (x, ypred ) 7→ ytrue
tory q(t) from this data. For this,
where ypred is the output of the implemented sensor

• Initialize the state of the Racer model with R.q(1)=MT_PI/2.;
model.
(lying down)
We take the output data file of the previous exercise as
• Assume the following simplified dynamic model:
 
the basis to learn this mapping.
0 I
A = I4 + τ  
2

 , a = 0 , Q = diag(10−6 , 10−6 , 1, 1)
0 0 Use multivariate linear regression, to compute such a
(6) linear map. See http://ipvs.informatik.uni-stuttgart
13.13.3 Lyapunov stability

de/mlr/marc/teaching/13-MachineLearning/02-regression.
pdf if you need details. Use
Recall that a general controlled dynamic system can be
β̂ ridge = (X⊤X + λI)-1 X⊤y
described with the Euler-Lagrange equation as
where y is a matrix, containing the multivariate output d ∂L ∂L ∂T ∂U
in each row; X is a matrix containing the multivariate Bu = − M q̈ + Ṁ q̇ −
= |{z} +
|{z} dt ∂ q̇ ∂q ∂q ∂q
control inertia
input (and an appended 1) in each row, and λ ≈ 10−4
| {z } |{z}
Coriolis gravity
is some small number.
a) What is the mean squared error of (ypred − ytrue )2 (not Consider a dynamic without Coriolis forces and con-
using the learned mapping) stant M (independent of a and q̇).
b) What is the mean squared error of (f (x, ypred )−ytrue )2 Can you give sufficient conditions on U (q) (potential en-
using the learned linear map f ergy) and u(q) (control policy) such that energy is a Lya-
punov function?
c) (Bonus) Can you weave in this learned mapping into
the Kalman filter of the first exercise?
13.14 Exercise 13
13.13 Exercise 12 On Wed. 29th we meet as usual for the exercise.
On Tue. 28th, 14:00 my office, interested students are

13.13.1 Controllability invited to try whatever they like on the racer hardware,
play around, etc.
Consider the local linearization of the cart-pole,
   
0 

1 0 0



0 

0


 0 0

0



 c1



13.14.1 Policy Search for the Racer
ẋ = Ax + Bu , A= 
  ,

B= 



0 


0 0 1





0 


 g   −c1 
0 
0 0  
4
3 l−c2
4
3 l−c2 We consider again the simulation of the racer as given
in 09-racer in your code repo.
Is the system controllable?
In this exercise the goal is to find a policy
π : φ(y) 7→ u = φ(u)⊤w
13.13.2 Stable control for the cart-pole
that maps some features of the (history of the) direct
sensor signals y to the control policy.
Consider a linear controller u = w⊤x with 4 parameters
w ∈ R4 for the cart-pole. Use black-box optimization to find parameters w that
robustly balance the racer.
a) What is the closed-loop linear dynamics ẋ = Âx of
the system? Some notes:
b) Test if the controller with w = (1.00000, 2.58375, 52.36463, 15.25927)

(computed using ARE) is asymtotically stable. What • Features: In the lecture I suggested that a range
are the eigenvalues? of interesting features is:

yt , ẏt , hyi0.5 , hẏi0.5 , hyi0.9 , hẏi0.9 , ut , ut−1
c) Come up with a method that finds parameters w
such that the closed-loop system is “maximally stable” However, I noticed that a balancing policy can
around x∗ = (0, 0, 0, 0) (e.g., asymptotically stable with also be found for the direct sensor signals only,
fastest convergence rate). that is:
φ(y) = (1, y) ∈ R5
Output the optimal parameters and test them on the
(the augmentation by 1 is definitly necessary).
cart-pole simulation you developed in exercise 9 (in course/07-cartPole).
• Cost function: Realistically, the running costs ct control). The pole itself cannot be articulated, only bal-
would have to be defined on the sensor signals anced using the arm. Above the arm (in the “ceiling”)
only. (On the real robot we don’t know the real is a hole. Initially the pole is hanging down; the prob-
state – if the robot is to reward itself it needs to lem is to swing up the pole and then balance it to in-
rely on sensor signals only.) I tried sert it in the ceiling’s hole without collision. We assume
costs += .1*MT::sqr(y(3)) + 1.*MT::sqr(y(2)); to have full access to the system dynamics in the form
q̈M (q)+F (q̇, q) = u, where q = (q arm , q pole ) contains the
which combines the wheel encoder y(3) and the
6 arm DoFs q arm ∈ R6 and the 2 pole DoFs q pole ∈ R2 .
gyroscope reading y(2). That worked mediocre.
Since we cannot actuate the pole directly, upole ≡ 0.
For a start, cheat and directly use the state of the
simulator to compute costs: If you would like to see the system, download the newest
costs += 1.*MT::sqr(R.q(0)) + 10.*MT::sqr(R.q(1)); version of the http://userpage.fu-berlin.de/˜mtoussai/source-code/
libRoboticsCourse.11.1.tgz and have a look at course/08-pin_ba
• Episodes and duration costs: To compute the
cost for a given w you need to simulate the racer For all questions: Write down the precise controller equa-
for a couple of time steps. For the optimization tions, or cost function, or constraints, or search/optimization
it is really bad if an episode is so long that it in- strategy, or whatever is necessary to precisely define
cludes a complete failure and wrapping around your solution.
of the inverted pendulum. Therefore, abort an
episode if fabs(R.q(1)) too large and penal- a) Hold steady: Let q arm ∈ R6 be the 6D arm joints
ize an abortion with an extra cost, e.g., propor- and q0arm their initial position. How can you hold the arm
tional to T −t. Try different episode horizons T up steady around q0 (counter-balance gravity and potential
to 500; maybe increase this horizon stage-wise. perturbations while the pole is hanging down)? Which
methods from the lecture do you use?
• Optimizer: You are free to use any optimizer you
like. On the webpage you find a reference imple- b) Swing up (1): How can you find a rough plan of how
mentation of CMA by Niko Hansen (with wrap- to swing up the pole? Which methods from the lec-
per using our arr), which you may use. In that ture do you use? (What does not work: using the local
case, add cmaes.o and search_CMA.o in the linearization of the pole around standing-up, because
Makefile. The typical loop for optimization is this linearization is totally wrong if the pole is hanging
SearchCMA cma; down.)
cma.init(5, -1, -1, -0.1, 0.1);
arr samples, values;
uint T=500;
c) Swing up (2): Given a rough plan of the swing up,
for(uint t=0;t<1000;t++){ is there a way to follow this rough plan directly? Alter-
cma.step(samples, values);
natively, how would you refine
for(uint i=0;i<samples.d0;i++) values(i) = evaluateControlParameters(samples[i], T); the plan to become an
uint i=values.minIndex();
cout <<t <<’ ’ <<values(i) <<’ ’ <<samples[i] <<endl; “optimal” swing up? Which methods from the lecture
}
do you use?
d) Balance steadily: When you managed to swing up

the pole, how could you balance it straight-up, even un-
der perturbations (Gaussian noise in the pole dynam-
13.15 Exercise 12 – SKIPPED THIS TERM ics)?
e) Pin-in-a-hole (1): How can you find a rough plan of

13.15.1 Balancing a pin in a hole with a torque- how to insert the balanced pole into the ceiling’s hole?
controlled arm
f) Pin-in-a-hole (2): Given that rough plan, how can you
follow this plan while balancing the pole in a stable way
Discplaimer: To actually solve this problem (in the sim- (robust under perturbations)? Alternatively, how can
ulator) is hard work and beyond the scope of this exer- you refine the plan to become more optimal?
cise. Instead, describe precisely how you would solve
the problem. g) Is there a way to solve the whole problem in a holis-
tic way, to find an “optimal” solution of the whole proce-
Consider a free swining pole (with a 2 DoF universal dure? (The swing-up already targets towards the ceil-
joint) at the tip of a 6 DoF arm. The arm joints can ing’s hole etc.) How could this be done realistically?
be articulated using torques (no direct position/velocity
h) Partial observability: Do you think the whole problem

could be solved if q pole is non-observable (the pole is
invisible, but the q arm are observed at every time step)?
How?
14 Topic list 14.2 Path planning
• Basics
This list summarizes the lecture’s content and is in-
– Path finding vs. trajectroy optimization vs. feed-
tended as a guide for preparation for the exam. (Go-
back control (3:5,6)
ing through all exercises is equally important!) Refer-
ences to the lectures slides are given in the format (lec- – Roughly: BUG algorithms (3:8-12)
ture:slide). – Potential functions, and that they’re nothing but
IK with special task variables (3:17)
– Dijkstra Algorithm (3:26,18-25)
14.1 Kinematics
• Probabilistic Road Maps (PRMs)
• 3D geometry – Definition (3:29,30) & Generation (4:31)
– Definition of an object pose, frame, transforma- – Importance of local planner (3:32)
tions (2:5,6)
– Roughly: knowing about probabilistic complete-
– Homogeneous transformations (4×4 matrix) (2:8,9)
ness (3:34)
– Composition of transformations, notation xW =
– Roughly: alternative sampling strategies (3:35)
TW →A TA→B TB→C xC (2:10)
• Rapidly Exploring Random Trees (RRTs)
• Fwd kinematics & Jacobian
– Algorithm (3:39)
– Fwd kinematics as composition of transforma-
tions (2:12) – Goal-directed (3:40) & bi-direction (3:42) exten-
sions
– Transformations of a rotational joint (2:13)
– Kinematic maps φpos : q 7→ y and φvec (2:15) • Non-holonomic Systems
– Definition of a Jacobian (2:17) – Definition of non-holonomicity (3:36) Be able to
– Derivation of the position and vector Jacobians give example
J pos , J vec (2:18,19) – Path finding: control-based sampling (3:52)
• Inverse kinematics (IK) – RRT extension for control-based exploration (3:57)
– Optimality criterion for IK (2:23,24) – Roughly: Intricacies with metrics for non-holonomic
systems (3:58-60)
– Using the local linearization to find the optimum
(2:26)
– Pseudo code of Inverse Kinematics control (2:28)
14.3 Dynamics
– Definition & example for a singularity (3:24)
• Motion profiles & Interpolation • 1D point mass & PID control
– Motion profiles (esp. sine profile) (2:32) – General form of a dynamic system (5:3)
– Joint space vs. task space interpolation of a – Dynamics of a 1D point mass (5:6)
motion (2:34,35) [e.g. using a motion profile in – Position, derivative and integral feedback to con-
one or the other space] trol it to a desired state (5:7,10,15)
• Multiple Tasks – Solution to the closed-loop PD system equa-
tions (5:11)
– How to incorporate multiple tasks (2:40,41)
– What are interesting task variables; know at – Qualitative behaviors: oscillatory-damped, over-
leads about pos, vec, align, and limits (2:46-52) damped, critically damped (5:13,14)
• Further • Euler-Lagrange equation
– Definition of a singularity (2:58) Be able to give – Definition (5:20)

example – Roughly: application to robotic systems (5:21)
– Be able to explain the consequences of the lo- Understand at least T = 21 q̇⊤M q̇
cal linearization in IK (big steps → errors) – Be able to apply on minimalistic system (5:22)
• Robot dynamics & joint/operational space control 14.5 Control Theory

– General form of the dynamics equation (5:28)
• Generally
∗
– Joint space control: given desired q̈ , choose – What we mean by “closed-loop system” (8:2)
u∗ = M (q) q̈ ∗ + F (q, q̇) (5:30)
– Topics in Control Theory (8:3)
– Operational space control: given desired ÿ ∗ ,
choose u∗ = T ♯ (ÿ ∗ − J˙q̇ + T F ) (5:31, 32, 34) • Optimal Control
– Joint space approach to follow a reference tra- – Definition of the (continuous time) optimal con-
ref
jectory q0:T (5:30) trol problem (8:9)
– Concept & definition of the value function (8:10)
– HJB equation (8:10)
14.4 Mobile Robotics – Infinite horizon → stationary solution (8:11)
– Awareness that optimal control is not the only
• Probability Basics approach; it shifts the problem of designing a con-
– Definitions of random variable, probability dis- troller to designing a cost function.
tribution, joint, marginal, conditional distribution, • Linear-Quadratic Optimal Control
independence (6:4-7)
– Definition of problem (esp. assumptions made)
– Bayes’ Theorem (6:7,8) (8:14)
– Continuous distributions, Gaussian, particles – Be able to express system dynamics in (locally
(6:9-13) linearized) standard form ẋ = Ax + Bu (8:19)
– Fact that the value function is quadratic V (x, t) =
• State Estimation
x⊤P (t)x (8:16)
– Formalization of the state estimation problem – Riccati differential equation (8:16)
(7:13)
– How P gives the optimal Linear-Quadratic Reg-
– The Bayes Filter as the general analytic solu- ulator (8:17)
tion (7:14,15)
– Algebraic (infinite horizon) Riccati equation (8:18)
– Gaussians and particles to approximate the Bayes
filter and make it computationally feasible: • Controllability
– Definition and understanding/interpretation of
– Details of a Particle Filter (7:22)
the controllability matrix C (8:27)
– Kalman filter (esp. assumptions made, not eq.
– Definition of controllability (8:26,27)
or derivation) (7:26)
– Be able to apply on simple examples (8:28)
– Extended KF (assumptions made) (7:28)
• Stability
– Odometry (dead reckoning) as “Bayes filter with-
out observations” (7:17,18) – Definitions of stability (8:34)
– What is smoothing (7:29) – Eigenvalue analysis for linear systems (8:35)

– Optimize controllers for negativity of eigenval-
• SLAM ues
– In what sense SLAM is a “chicken or egg prob- – Definition of a Lyapunov function (8:39)
lem” (7:34) – Lyanunov’s theorem: ∃ Lyapunov fct. → stabil-
– Joint inference over x and m: Extended Kalman ity (8:39)
SLAM (7:37) – Energy and value function as candidates for a
– Particle-based SLAM (map belief for each par- Lyapunov fct. (8:41,42)
ticle) (7:38-41)
– Roughly: graph-based SLAM & loop closing

(7:44-46)

13 Robotics Script

Uploaded by

Copyright:

Available Formats

13 Robotics Script

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

13 Robotics Script

Uploaded by

Copyright:

Available Formats

Introduction to Robotics

10 Reinforcement Learning in Robotics 55

11 SKIPPED THIS TERM – Grasping (brief intro) 61

12 SKIPPED THIS TERM – Legged Locomotion (brief intro) 64

– Noel Sharkey’s articles on robot ethics (Child care robots PePeRo..

AI in the real world

AI: Machine Learning, probabilistic reasoning, optimization

(robot “wife” aico)

• Planning & optimization

Tunicates digest their brain once they settled! gramming

• Motion was the driving force to develop intelligence

Robotics as intelligence research goal: localize and map yourself

History reducing real-world problems to mathematical problems

that can be solved efficiently

• Kinematics & Dynamics Craig, J.J.: Introduction to

Kinematic map, Jacobian, optimality principle of inverse kinematics, singularities, configura-

An advanced text book on planning is this:

Steven M. LaValle: Planning Al-

• Handbook of Robotics (partially online at Google books) http://tiny.

• Secretary, admin issues:

• Rules for the tutorials:

2 Kinematics Pose (position & orientation)

Kinematic map, Jacobian, inverse kinematics as optimization prob-

• Two “types of robotics”:

Frame and coordinate transforms

• Move all joints in a coordinated way so that the endeffector

• Basic 3D geometry and notation 2:6

• Translation and rotation: xA = t + RxB Kinematics

• Homogeneous transform T ∈ R4×4 :

TAB = R t joint B'

• A kinematic structure is a graph (usually tree or chain)

• TAB describes the translation and rotation of frame B relative

nates contra-variant. See “geometry notes” or Wikipedia for axis:

more details, if you like. 1

others: screw (1dof), cylindrical (2dof), spherical (3dof), univer-

Kinematic Map • The i-th joint is

vector aeff = φvec

Jacobian for a rotational joint

→ the whole sub-tree ro-

δpeff = [ai × (peff − pi )] δqi

2) A direction v ∈ R3 attached to the endeffector transformed to ⇒ PositionJacobian: 

• To compute the Jacobian of some endeffector position or vector,

J(q) = φ(q) =   1 ∂q2 

can generate the desired y ∗

Inverse Kinematics as optimization problem • We can derive the optimum as

f (q) = ||φ(q) − y ∗ ||2C + ||q − q0 ||2W

“Small step” application

• This approximate solution to IK makes sense

q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W qt+1 = qt + J ♯ (yt+1

• The formulation of IK as an optimization problem is very power- 2:27

ful and has many nice properties

01-kinematics: ./x.exe -mode 2/3

Solution using the local linearization Two additional notes

= q ′ + J ♯ (y ∗ − y ′ ) + (I − J ♯ J) h , h = q0 − q ′ (1) Motion profiles

Where are we? 2:33

• In the remainder: f (qT ) = ||qT − q0 ||2W/T + ||yT − φ(qT )||2C

Heuristic motion profiles qt = q0 + MP(t/T ) (qT − q0 )

Heuristic motion profiles Task space interpolation

• Assume initially x = 0, ẋ = 0. After 1 second you want x =

2) Project y0:T to q0:T using inverse kinematics: