Optimal Control Techniques
for Dynamic Walking
Katja Mombaur & Martin Felis
Optimization in Robotics & Biomechanics
IWR, University of Heidelberg
Presentation partly based on slides by
Sebastian Sager, Moritz Diehl and Peter Riede
Dynamic Walking 2011
Jena
Why do we do this tutorial?
• There are a lot of optimal control problems in walking robots
• There are often statements like
„We use SNOPT (fmincon, .... etc.) to optimize robot motions“
There is a lot more to optimal control than to
nonlinear optimization !!
Katja Mombaur & Martin Felis
Introduction
Katja Mombaur
Nonlinear Optimization Problem
• Famous nonlinear example function
– Rosenbrock function
Fletcher
Banana valley
function
Katja Mombaur & Martin Felis
Important property of
nonlinear optimization problems
• The optimization is performed in finite dimensional space
• The result is a point in finite dimensional space
• This will change for the next problem class
Katja Mombaur & Martin Felis
Optimal control problems
Optimal control = Optimal choice of inputs for a dynamic system
Katja Mombaur & Martin Felis
Optimal control problems
• Optimization problems
including dynamics
e.g. optimal control problems
• State and control variables
are functions in time
(infinite-dimensional
variables)
Katja Mombaur & Martin Felis
Optimal control methods
Transformation
from optimal control
to nonlinear optimization
problem
Optimal control
method
Nonlinear Optimization
Methods
Katja Mombaur & Martin Felis
MUSCOD – an efficient tool for
optimal control problems
• Developed by Bock and co-workers (IWR, University of Heidelberg)
• Original version Muscod I 1981 (Fortran 77)
• MUSCOD II developed in 1995 ( C )
• Since then continuous extensions and updates (e.g. Mixed integer optimal
control, NMPC, ...)
• also contains efficient integrators with sensitivity generation
• Unfortunately not yet publicly available, but maybe in the near future
• If you are interested, contact us
Katja Mombaur & Martin Felis
Optimal Control
Katja Mombaur & Martin Felis
Optimal control problem (simplifed)
Lagrange type Mayer type
Objective
function
Process
dynamics
Initial and final
constrainte
Katja Mombaur & Martin Felis
Optimal control problem (more complex)
e.g. for the optimization of dynamic robot gaits:
Multiphase problems with many additional constraints
But we will stay with the simple problems for now
Katja Mombaur & Martin Felis
3 different approaches for
optimal control problems
Reminder – Key problem:
How to handle infinite dimensionality of states x(t) and controls u(t)?
• Dynamic Programming / Hamilton-Jacobi-Bellman equation
• Indirect Methods/ calculus of variations/ Pontryagin Maximum Principle
Only approach
• Direct Methods (discretization of controls) treated in this talk
Katja Mombaur & Martin Felis
Direct methods for optimal control problems
• Common idea: replace control functions u by a discretization
(= a finite-dimensional parameter vector)
• Are also called First-dicretize-then-optimize
methods
Infinite dimensionality of controls resolved
But what about states x(t)?
Katja Mombaur & Martin Felis
Three different methods for state discretization
– Direct Collocation
– Direct Single Shooting
– Direct Multiple Shooting
Katja Mombaur & Martin Felis
Direct Single Shooting
Katja Mombaur & Martin Felis
NLP in Direct Single Shooting
Katja Mombaur & Martin Felis
Numerical example
Katja Mombaur & Martin Felis
Single shooting optimization for x0 = 0.05
Katja Mombaur & Martin Felis
Single shooting iteration
Solution
Katja Mombaur & Martin Felis
Direct Single Shooting: Pros and Cons
+ Concept easy to understand
+ Can use state-of-the art ODE or DAE integrators
+ (comparatively) Few optimization variables even for large ODE/DAE
systems
+ Need only initial guess for controls q
- Can not use knowledge of x in initialization (e.g. in tracking problems)
- Often does not work when initial guesses for controls are far off
trajectory „explodes“
These drawbacks
- Unstable systes are difficult/impossible to treat are adressed by
Multiple Shooting
Often used in enigineering applications, e.g. in packages gOPT (PSE),
ADOPT (Marquardt), ...
Katja Mombaur & Martin Felis
Direct Multiple Shooting
Bock, Plitt, 1981
• Discretize controls piecewise on
a coarse grid
• Use functions which have only local
support, e.g. piecewise constant
or piecewise linear functions
Katja Mombaur & Martin Felis
Direct Multiple Shooting
Bock, Plitt, 1981
– Split long integration interval into many shorter ones
– Use initial values of all integration intervals as free variables
and „shoot“ a new integration from each initial value
Katja Mombaur & Martin Felis
Direct Multiple Shooting
Bock, Plitt, 1981
– Introduce constraints to close gaps („continuity conditions“)
– Also use integators to compute objective function
Katja Mombaur & Martin Felis
Discretized Optimal Control Problem
Katja Mombaur & Martin Felis
How to solve the NLP
Katja Mombaur & Martin Felis
The Lagrangian Function for NLP
Lagrangian function of the constrained NLP
• equality multipliers λi may have both signs in a solution
• inequality multipliers µi cannot be negative (cf. shadow prices)
• for inactive constraints, multipliers µi are zero
Katja Mombaur & Martin Felis
First order optimality conditions
Karush-Kuhn-Tucker necessary conditions (KKT-conditions):
• x* feasible
• there exist λ*, µ* such that
• and it holds the complementary condition
i.e. µi*= 0 or hi (x*)= 0 for all i
Katja Mombaur & Martin Felis
Newton‘s method for unconstrained NLP
Idea: calculate zeros of the equation
to satisfy first order optimality constraints
Taylor series:
Newton Iteration:
Inverse of Hessian Gradient
of Lagrangian function
Katja Mombaur & Martin Felis
SQP - applying Newton‘s method to KKT
Newton‘s method finds zeros of nonlinear equations
Katja Mombaur & Martin Felis
SQP – sequential quadratic programming
• This is equivalent to solving the following quadratic programming problem
(QP) in each step of the SQP
Hessian of
Lagrangian function
• use active-set-strategy to determine active inequality constraints
• Practical SQP:
• use update formula for Hessian
• use step size adaption
Katja Mombaur & Martin Felis
Example - Newton‘s method
• Behavior for banana valley function
Katja Mombaur & Martin Felis
Example - comparison between methods
• Newton‘s method much faster than steepest descent method
• convergence roughly linear for first 15-20 iterations since step length
αk <1
• convergence roughly quadratic for last iterations with step length
αk ≈1
Katja Mombaur & Martin Felis
Essential steps in NLP solution
• Sequential quadratic programming (SQP)
methods are very efficient methods to solve
nonlinear programming problems
• Use a QP approximation to the NLP at
every iteration
• SQP = Newton‘s method applied to KKT
conditions
• Reduction of computation times by
computation of Hessians via update formula
• Globalization of convergence by using
damped Newton steps
• Important to note: iterates can be infeasible,
only solution must be feasible
Katja Mombaur & Martin Felis
Back to the optimal control
problem
Katja Mombaur & Martin Felis
Direct multiple shooting
Bock, Plitt, 1981
• Simultanuous optimization and simulation
Discretized optimal control Initial value problem solution
problem (Integration + derivatives)
(NLP) on each
multiple shooting interval
si, ui, p, h
Variables:
discretized states si,
discretized controls ui, xe,i + sens. xe,i
parameters p,
Phase durations hi si
Solution by efficient integrators
Solution by efficient SQP
Katja Mombaur & Martin Felis
Trajectory sensitivity generation:
black box - a bad idea
Katja Mombaur & Martin Felis
Trajectory sensitivity generation:
IND – a good idea!
• IND = internal numerical differentiation (Bock, 1981)
Katja Mombaur & Martin Felis
Direct Multiple Shooting
Bock, Plitt, 1981
• Result of discretization:
Large-scale nonlinear programming problem (NLP)
• Special structure originating from discretization (in KKT matrix)
Hessian
of Lagrange Fct
•
Constraint
Jacobians
•
MUSCOD
• Solution with structure-exploiting, tailored sequential quadratic programming
(SQP) method, special condensing techniques
Katja Mombaur & Martin Felis
Direct Multiple Shooting example
• Same example as before; same initialization : u = 0
– 3 iterations compared to 7 for single shooting
Katja Mombaur & Martin Felis
Direct Multiple Shooting: Pros and Cons
Katja Mombaur & Martin Felis
Demonstration of example
optimal control problem solution
with MUSCOD-II
A simple example
Katja Mombaur & Martin Felis
A simple optimal control problem
• A car is supposed to travel 300 m in 32 seconds
• Its initial and final velocity are zero
• The acceleration can vary between -2 m/s2 and 1m/s2
• The maximum velocity is 30 m/s
• Optimization goal: minimize energy in terms of accelrations squared
Katja Mombaur & Martin Felis
A simple optimal control problem
• Mathematical formulation:
Objective function
(Lagrange type)
lfcn
subject to the constraints:
Dynamic process
model
ffcn
Initial and
final constraints
rdfcn
Bounds
(data file)
Katja Mombaur & Martin Felis
MUSCOD-II Model file
• Macros
1 # of model stages
0 # of global parameters
0 # coupled constraints
0 among them, # of equaliy cnstraints
2 # of differential state variables
0 # of algebraic state variables
1 # of controls
0 # of local parameters
2 # of start point constraints
2 among them, # of equality constraints
2 # of end point constraints
2 among them, # of equality constraints
Katja Mombaur & Martin Felis
MUSCOD –II source file
• Objective function: Integral term, Lagrange type
• Dynamical equations (right hand side)
Katja Mombaur & Martin Felis
MUSCOD –II source file
• Constraint equations
Katja Mombaur & Martin Felis
MUSCOD-II source file
// Define optimal control problem
//Define model dimensions
// Define model stage with index, dimensions of
// state and control variables, pointers to
// right hand side and objective fnctions, ... etc.
Katja Mombaur & Martin Felis
MUSCOD-II source file
// Define multipoint constraints
(at start point, interior points or end point (of last phase)
Katja Mombaur & Martin Felis
MUSCOD-II data file
• Define number of multiple shooting intervals (also equals number of control
intervals)
• Define start values, min and max values, and scaling factors for phase time
flag to fix variable
Katja Mombaur & Martin Felis
MUSCOD-II data file
• Define type of state initialization:
s_spec=
0: every point
1: start value generation by linear interpolation
2: start value generation by forward integration
• Define start values, scaling factors,
min and max values for states
Katja Mombaur & Martin Felis
MUSCOD-II data file
• Type of control discretization
u_
• Control variable start values, scaling, min and max
Katja Mombaur & Martin Felis
MUSCOD-II data file
• Scaling of constraints:
• Scaling, min and max of objective function
Katja Mombaur & Martin Felis
Results
Katja Mombaur & Martin Felis
Same problem with different objective function
• Mathematical formulation:
Objective function
(Mayer type)
mfcn
subject to the constraints:
Dynamic process
model
ffcn
Initial and
final constraints
rdfcn
Bounds
(data file)
Katja Mombaur & Martin Felis
Results for time minimization
Katja Mombaur & Martin Felis
Demonstration of example
optimal control problem solution
with MUSCOD-II:
A bipedal running robot
Katja Mombaur & Martin Felis
A bipedal running robot
• Running motion: alternation between single leg contact phases and flight
phases
Phase 1: Phase 2:
Contact Flight + Leg
shift
Touch Take- Touch
-down off -down
Leg1 Leg2
Katja Mombaur & Martin Felis
A bipedal running robot
5 DOF in flight
4 DOF in contact 4 actuators :
trunk
Upper legs
lower legs Only active in
(massless) contact phase
Katja Mombaur & Martin Felis
Model of bipedal running robot
• Flight phase
Katja Mombaur & Martin Felis
Contact phase
• Spring damper forces
• Coupling between state variables
Katja Mombaur & Martin Felis
Model of bipedal running robot
• Contact phase
Katja Mombaur & Martin Felis
Form of model equations
required for optimal control code MUSCOD
.
x = ffcn
where a is the solution of
see previous
slides
also satisfying
Katja Mombaur & Martin Felis
Switching functions
• Switching function for lift-off:
Leg spring
at rest length
• Must also satisfy condition:
Positive vertical
velocity (moving up)
• Switching function for take-off:
Height of contact
point = zero
• Must also satisfy condition:
Negative vertical
velocity at contact
point
Katja Mombaur & Martin Felis
Touchdown discontinuity & Periodicity
• Inelastic impact: there is a jump in all model velocities at touchdown
compute velocity after touchdown:
• Periodicity constraints on all positions (except forward direction)
and velocities
Katja Mombaur & Martin Felis
Katja Mombaur & Martin Felis
The same robot performing summersaults...
Mombaur et al.
2005
Only requires modification of periodicity constraints:
+ 360 ° in torso and leg angles per step
Katja Mombaur & Martin Felis
Katja Mombaur & Martin Felis
... and even open-loop stable flip-flops
Requires another modification of periodicity constraints:
+ 180° in torso and leg angles per step
Katja Mombaur & Martin Felis
Katja Mombaur & Martin Felis
Literature
Katja Mombaur & Martin Felis
Thank you for your attention!
www.orb.uni-hd.de
Katja
Pictures taken at the Mombaur
Musée de l‘Automate, Souillac
