Convex and Combinatorial Optimization for Dynamic Robots in the Real World

Russ Tedrake

russt@mit.edu
groups.csail.mit.edu/locomotion

My motivating applications

Robots in the home
Automous vehicles
Approaches today are (surprisingly?) ad-hoc.
I believe in formal (model-based) design/analysis.

My goals for today

  • Discuss some of the big questions
    • Do home robots actually need "verification"?
      (or should be all be working on batteries, or sensors, or actuators ...)
    • What are the relevant challenges/technologies for autonomous cars?
  • Discuss what dynamic robots can and can't do well
  • Describe the optimization problems we formulate and how we solve them
  • Expose our "pain points"...
    and convince you to help!
  • Organize around core problems:
    • Perception, Planning, Feedback Design, Verification

"Hybrid Systems"

  • Not just discrete decisions; also discrete/discontinuous dynamics (e.g. collision events and frictional contact).
  • "Language" of hybrid systems (modes, guards, resets, ...) is sufficient...
  • ... but often impoverished for synthesis/verification.
  • Found happiness in e.g. measure-differential inclusions (MDIs) and mixed combinatorial-continuous optimization

"Hybrid Systems"

  • Not just discrete decisions; also discrete/discontinuous dynamics (e.g. collision events and frictional contact).
  • Constant theme of "exploiting structure" (especially between modes)
    • Nonlinear dynamics: sparsity, convexity, algebraic structure.
    • Combinatorial structure (even in the mechanics).
    • Approaches: Submodularity/Matroid, SMT, MI-Convex, Convex relaxations, ...
How well do (humanoid) robots work?
Can the tools from the HSCC community help?

MIT's approach to the DARPA Robotics Challenge

Rules allowed a human operator, but with a degraded network connection

Teleoperation Complete Autonomy

Our approach:

  • Focus on autonomy.
    • Human operator provides high-level ``clicks'' to seed (nearly) autonomous algorithms
  • Almost everything the robot does is formulated as an optimization
    • For me personally, a triumph for optimization-based synthesis

Trajectory Motion Planning

Simplest example: Inverse Kinematics

    Joint positions: $q$
    \[ \min_{q} |q-q_{desired}|^2 \]
    subject to
    • end effector (rich constraint spec)
    • joint limits
    • collision avoidance
    • "gaze constraints"
    • feet stay put
    • balance constraints (center of mass)
    • ...
    Solved with sequential quadratic programming (SQP)
    Commercial solver: SNOPT
    Analytical gradients
    Exploit kinematic chain sparsity
    around 100 Hz
    Returns locally optimal $q$
    (and possibly "infeasible")

Big robot, little car

Kinematic trajectory planning

  • Optimize $q(t)$ spline coefficients
  • Add constraints through time
    (points in contact don't move)
  • Solve with SQP
    (or randomized planning)
  • Solves in seconds

Dynamic trajectory planning

For more dynamic motions have to add additional constraints ($F=ma$, actuator limits, friction cones, ...)

For smooth dynamical systems, trajectory optimization still works well

Dynamic trajectory planning

For more dynamic motions have to add additional constraints ($F=ma$, actuator limits, friction cones, ...)

For smooth dynamical systems, trajectory optimization still works well

Planning with Contact

  • Contact (e.g. foot falls or grasping) adds "complementarity constraints"
    • distance to contact > 0   OR   contact force > 0
  • Add nonlinear constraint to SQP:
    distance $\times$ force = 0

Planning with Contact

  • Contact (e.g. foot falls or grasping) adds "complementarity constraints"
    • distance to contact > 0   OR   contact force > 0
  • Add nonlinear constraint to SQP:
    distance $\times$ force = 0
  • Plans take > 1 minute to compute
  • ...and don't always succeed (local minima, numerical issues, ...)
  • Did not use in the competition

absolutely awesome! but no planning here (yet)

There is real-world, online planning here:

but do you know why humanoids walk like this?

Constant center of mass (COM) height $\Rightarrow$

  • No impact events
  • Linear center of mass dynamics
$\Rightarrow$ COM planning (given footsteps) as a convex optimization
  • Standard recipe for humanoid walking:
    1. plan footsteps/contacts [heuristic]
    2. plan constant-height CoM (also zero angular momentum) [convex]
  • My goal: reliable online planning that
    • includes footsteps, contact
    • removes CoM / momentum restrictions

Footstep planning

Footsteps add a combinatorial aspect to the planning problem:

  • Left foot or right foot?
  • Cinderblock A or block B?
  • Some search / planning feels inevitable

But there is a continuous portion, too

  • Given block A, where exactly do I put my foot?
  • Motion of center of mass / joints
  • ...

Planning on LittleDog (c. 2008)

Search-based footstep placement

image from Zucker et al., IJRR, 2011
  • Chestnutt et al., Veranza et al., Winkler et al., Zucker et al.,...
  • Search over action graph of either
    • Footsteps
    • Body positions
  • Discrete set of reachable footsteps
  • No guarantees of completeness
  • Adding dynamics made the search harder (more expensive checks)
Dynamics adds constraints that should guide the search.
One solution: mixed-integer convex optimization.

First step: explicitly enumerate the regions

Mixed-integer convex optimization

  • Assignment of contacts to regions adds an "integer constraint":
    • \begin{align*}\quad \\ \minimize_x \quad f(x) \\ \text{subject to} \quad g(x) \le 0 \\ x_i \in \mathbb{Z} \\ \quad \end{align*}
  • Non-convex optimization (always).
  • Worst-case complexity is awful. Brute force search.
  • "Mixed-integer convex" iff relaxation (ignoring integer constraint) is convex
  • Relaxation gives lower bounds $\Rightarrow$ effective branch-and-bound search
  • Very efficient commercial solvers.
    • return global optimality (to tolerance)
    • or "infeasible"

More than a "hybrid system"

  • Both AI-style search and "hybrid systems" model promote thinking about every mode as independent.
  • Transcription to MI-Convex forces thinking about the relationship between modes.
  • Here:
    • Always $F=ma$,
    • Non-zero force from a region when foot is in contact.

Adding more dynamics

  • Still very conservative walking
  • Integer variables enabled making/breaking contact
  • General case needs to handle angular momentum (but non-convex)
\begin{align} T = \mathbf{p} \times \mathbf{f} \\ = p_{z}f_{x} - p_{x}f_{z}\\ \end{align}
  • Foot position relative to CoM: $\mathbf{p}$
  • Contact force: $\mathbf{f}$
  • Moment about CoM: $T$

Momentum terms are non-convex

$$T = pf$$
  • Original non-convex surface
  • $p$ is bounded (footstep constraints)
  • $f \ge 0$ bounded by actuators
  • Idea: relaxation using integer variables

MI-Convex relaxations of bilinear terms

$$T = pf$$
  • Linear programming relaxation (McCormick Envelope)
  • Four linear constraints
  • Can be tight for small contact regions

MI-Convex relaxations of bilinear terms

$$T = pf$$
  • Piecewise McCormick Envelope (PCM)
  • Tighter relaxation
  • Adds integer variables
  • Here continuous $\Rightarrow$ discrete optimization
  • Also exploring discrete $\Rightarrow$ continuous (e.g. SDP relaxation)

Application to a bounding planar quadruped

Application to a bounding planar quadruped

  • Three contact regions (bold lines)

Application to a bounding planar quadruped

  • Three contact regions (bold lines)
  • Three (overlapping) free-space regions (shaded)

Application to a bounding planar quadruped

  • Three contact regions (bold lines)
  • Three (overlapping) free-space regions (shaded)

Application to a bounding planar quadruped

  • Three contact regions (bold lines)
  • Three (overlapping) free-space regions (shaded)

Application to a bounding planar quadruped

  • Assignment to regions provides (convex) constraints for position and force

Application to a bounding planar quadruped

From MICP results to whole-body planning

  • Optimization on simple models can seed non-convex optimization

Grasp optimization

  • Optimize forces and contact positions for robustness
  • Non-convex from torque (cross product) + assignment of fingers to regions
  • Bilinear Matrix Inequalities (BMIs), solved as SDP w/ rank-minimization
  • Include kinematic and dynamic constraints (solves inverse kinematics, too)

Grasp optimization

  • Optimize forces and contact positions for robustness
  • Non-convex from torque (cross product) + assignment of fingers to regions
  • Bilinear Matrix Inequalities (BMIs), solved as SDP w/ rank-minimization
  • Include kinematic and dynamic constraints (solves inverse kinematics, too)

Grasp optimization

  • Find pose to maximize wrench disturbance given torque limits

Feedback Control

Example: Whole-Body Force Control

  • Planners give desired CoM + end effector trajectories
  • Solve differential Riccati equation for approximate CoM feedback
  • One-step control Lyapunov function accounts for:
    • whole-body inverse dynamics
    • instantaneous contact constraints (incl. friction limits)
    • torque limits
  • results in a sparse QP

Whole-Body Force Control

  • Balance control worked extremely well...

Whole-Body Force Control

  • Balance control worked extremely well...

Whole-Body Force Control

  • Balance control worked extremely well...

Whole-Body Force Control

  • Balance control worked extremely well...
  • ... except for the one time it didn't

What went wrong?

  • First, a human-error
  • Then tailbone hit the seat and feet came off the ground...
  • No contact sensor in the butt
  • Dynamics model very wrong
  • State estimator very confused
  • Controller is done!

This was a "corner case"

  • Errant process sending commands to the ankle
  • Particularly unfortunate ankle command
  • When robot was most vulnerable
  • Never saw this during testing
Corner cases are a fundamental research challenge.

Ironically

  • For flat terrain, we did work out good heuristics...

Ironically

  • For flat terrain, we did work out good heuristics...

Another form of robustness

Feedback for Grasping

  • Grasping was open-loop (no feedback)

Feedback for Grasping

  • Grasping was open-loop (no feedback)
  • Touch sensors / hand cameras were fragile and difficult to use

Feedback for Grasping

  • Have been trying to extend grasp optimization to feedback design
  • Adds sums-of-squares constraints
  • Still numerically bad. No success here, yet.

Perception

Continuous walking with dense stereo

Perception for Walking

  • In the competition, relied on lidar; local "fitting" via least squares was sufficient

Perception for multi-contact planning

Perception for multi-contact planning

also manipulation of unknown objects

  • In the DRC, we only touched known objects (cinderblocks, drill, ...)
  • Fit CAD models to raw sensor data

Perception for Manipulation

  • Avoided object recognition via a few clicks from the human

Perception for Manipulation

  • Avoided object recognition via a few clicks from the human

Robust Synthesis/Verification

To what extent can we address the limitations with better formal synthesis?
    Two (related) ideas:
  • Reduce sensitivity of components to expected disturbances (e.g. $L_2$-gain minimization, robust control).
  • System-level search for logical corner cases (e.g. formal methods).
Have been extending robust control formulations to handle
complex robot dynamics
Good progress on (aggressive) vehicle dynamics

(dynamics of vehicles are simpler than humanoids)

Example: An airplane that can land on a perch

Can we make a control system for a fixed-wing airplane to land on a perch like a bird?

Glider Perching Experiment

  • Flat-plate glider (no propellor)
  • Off-board sensing and control
  • System identification to find ODE model of post-stall flow
  • Robust planning and control

Robust planning and control

    Key ingredient: Reason algebraically about nonlinear system dynamics
    • Motion planning focuses on a single trajectory (one solution)
    • Robust analysis reasons about all possible solutions

    Analysis by searching for a Lyapunov function.

Computing Lyapunov functions

  • Historically difficult to find Lyapunov functions
  • First observation:

    • Verifying Lyapunov conditions requires checking positivity of functions $(V(x)>0, \dot{V}(x) < 0)$

  • Second observation: The dynamics of my robots are (rational) polynomial
  • "Sums-of-Squares" (SOS) optimization can approximate positivity as a semidefinite constraint
    • Verifying a candidate Lyapunov function is a convex optimization
    • Robust control design using Lyapunov functions is a bilinear optimization
    (results by Pablo Parrilo, Ufuk Topcu, Andy Packard, ...)

Example: Region of attraction for a Quadrotor

Lyapunov verification of (aggressive) motion plans

  • 7+ dimensional state space
  • Exhaustive evaluation is impossible
Trajectories can't be robust.
Funnels can.

Flying through forests

experiments by Andy Biewener et al at Harvard

Flying through forests

Real-time planning w/ funnels

    What if obstacles aren't known until runtime?

    • Real-time Sums-of-Squares?
    • Precompute library of parameterized funnels
    • Shift/compose funnels at runtime

Outdoor experiments using 120 fps onboard vision

34 inch (86 cm) wingspan. 25 mph (>11 m/s)
Vision system works.
But we have zero formal results once there is perception in the loop.
  • Existing theory/algorithms only got us so far
  • How many hours could you put on the robot?
    • "Build it and break it"
  • Logistics: corner cases mean human intervention, loss of efficiency
  • Home robots: "artificial stupidity" will limit consumers' appetite
  • Autonomous vehicles: corner cases will cost human lives

But why do robots really need "verification"?

To accelerate the pace of innovation.

It takes time to find the corner cases

source: A Statistical Analysis of Commercial Aviation Accidents 1958-2015, Airbus.

  • Challenge: Increasing system complexity
    • Sensors (camera, lidar, radar, projected light...)
    • Dynamics (contact mechanics, tire models, ...)
    • Software (perception, planning, learning in the loop)
    • Complexity of the environment (with humans)
Question:
  • How do you provide test coverage of every home in the U.S.?
  • ... and every possible human interaction?
Hint: You don't.
Have to play the statistics game.
But experiments are expensive, and data is precious.

Data is precious

I'm taking a different approach

  • Open-source C++ toolbox for model-based optimization (http://drake.mit.edu)
  • Simulate very complex dynamics, sensors, ... plus tools for
    • System identification
    • Runtime monitoring / anomaly detection
    • Active experiment design
  • Built for optimization-based design/analysis
    • Cloud simulation (e.g. for black-box approaches)
    • Provides analytical gradients, sparsity, polynomials, ...
    • Bindings to MATLAB, Python, Julia, ...
    • Clear separation between problem formulations and solvers
  • Toyota Research Institute + other industry investment
    • Very interested in robustness / verification
    • Immediate applications

Rigorous Simulation

The fallacy of the “simulation gap” in robot manipulation
Have physics-based simulations that will highlight the inadequacy
Simulations are deterministic, repeatable, and provide perfect ground truth

Rigorous Simulation (on the cloud)

Specific technical challenges

  1. Robust design/analysis through contact
    • Current controllers have "obvious" deficiencies w/ respect to contact uncertainty.
  2. Robust perception
    • Really feedback systems with perception in the loop.
    • Don't even have the robustness metric yet (e.g. input-output gain less important than outlier rejection)
  3. Robust design/analysis w/ humans in the loop.
    • Modeling is the key challenge.

Robustness design/analysis through contact

Combinatorial number of contact conditions

  • Key result: (tight) relaxation with polynomial number of constraints.
  • Still big optimizations. Poorly conditioned (in the default parameters).
  • Simple models so far. Need to exploit more of the structure in the polynomial equations.

    • Robust synthesis/analysis through contact

    Robust design/analysis with humans in the loop

    Building large-scale traffic models (+ sensor models) in drake.

    Work with Jon DeCastro and Soonho Kong (in the audience).

    Emerging themes:

    • Falsification, not verification
    • Falsification alone is too easy (adversarial vehicles will get you).
      • Need good models, return highest probability failures until satisfied

    Summary

    • Planning
      • Mostly nonlinear optimization
      • New results getting more dynamics into mi-convex formulations
    • (Low-level) Perception
      • Was about appearance. Now about geometry.
      • Mostly simple (unconstrained) optimization, with randomized restarts
      • Essentially no guarantees. Still not robust
    • Feedback/Verification
      • One-step control-Lyapunov as a QP, effective but limited
      • SOS verification/synthesis works well for smooth systems, even aggressive motion
      • SOS through contact still numerically difficult

    Summary

    Do robots/vehicles need HSCC?
    Yes, but "safety-critical" might not be a sufficient motivation.
    Confluence of (software-)engineering best practices with
    model-based design/analysis to accelerate pace of iteration.
    Verification may be a fools-errand, but
    falsification can give value immediately.

    Summary

    I believe in the key is discovering/writing down the "right" models,
    in a language that can expose their structure
    and developing advanced algorithms to leverage that structure.
    (current obsession is on combinatorial+continuous optimization
    with SMT, SDP, submodular/matroid methods, mi-convex, ...).

    For more information

    • Positions available!
      • Research and Software Developer Positions at Toyota Research Institute
      • Postdoc openings in my group at MIT