Whole-body Planning and

Stabilization of Dynamic Motion with

Contact
Michael Posa*, Scott Kuindersma, and Russ
Tedrake*
*MIT and Harvard
Dynamic Walking 2015

Whole-body Planning and

Control
Goal: efficient motions, periodic and
aperiodic
DRC demonstrated capability of QP
controllers
Largely ZMP/COM-based
Flat foot walking

Build on classical optimal control

and stabilization algorithms
Integrate QP controllers with state
of the art trajectory optimization
2 of 13

Contact Implicit Trajectory

Optimization

Find dynamically feasible trajectory

Minimizes cost (e.g. effort or cost of transport)
Without a priori specification of contacts

Jointly optimize over state, input, and

contact
forces
Gap
function

Floating base
Positions and
Velocities

What about
executing the
trajectory?

Dyn. Walking 2012, WAFR 2013, IJRR 2014]

3 of 13

Trajectory Accuracy
Contact implicit method is O(h)
Higher accuracy methods for smooth
systems
Direct collocation O(h3) [Hargraves and
Paris,1987]

Extend direct collocation to handle contact

constraints
Floating base coordinates
Closed kinematic chains (e.g. double support)
Friction limits

Trajectories constrained to contact manifold

4 of 13

Direct Collocation O(h3)

Optimize over
Compute
Construct cubic splines between
Match dynamics with spline derivative at
midpoints

5 of 13

Nave Approach
Compute constrained dynamics
Add constraints for state to be on
manifold
Overconstrained!
Consider the unactuated case: given
find

6 of 13

Constrained Direct
Collocation

Optimize over
Compute
Constraints

Constrained
Dynamics
Knot points on
manifold

Spline matches dynamics at collocation

point
up to a projection onto contact
manifold

7 of 13

Stabilization: Constrained
LQR
Linearize deviations from nominal

Dynamics satisfy (linearized) manifold

constraint
Can only control in the kernel of
Suppose
an orthonormal basis of
kernel
Then
is controllable
with
8 of 13

Stabilization: Quadratic
Programming
Descend LQR cost-to-go
Optimize over accelerations, control
input, and contact forces
Subject to linearized constraints:
Whole-body dynamics
Contact manifold
Coulomb friction limits
Input saturations

Kuindersma et al., ICRA 2014]

9 of 13

Example: Biped Walking

10 of 13

Example: Monopod Hopper

11 of 13

Example: Underactuated
Biped

12 of 13

Conclusion
Explicitly treating contact forces a natural
extension to traditional algorithms
Future work
Lateral stabilization and 3D
Atlas experimentation
Contact implicit direct collocation
Higher order accuracy without a mode sequence
Scott is looking for postdocs and students to join the
Harvard Agile Robotics lab, talk to him or email
scottk@seas.harvard.edu
13 of 13