Simulation and Optimization through Contact

(should we use analytical gradients?)

Russ Tedrake

Slides available at https://slides.com/russtedrake/iros-contact

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases
  • Stochastic optimal control can smooth discontinuous objectives
  • ... but Monte-Carlo with analytical gradients of discontinuous/stiff objectives can give bad results
    • might explain the relative success of zero-order methods
    • new algorithms based on "randomized smoothing"
  • Beware of artificial discontinuities due to subtle interactions between the collision and physics engines
    • "Hydroelastic" contact models offer a solution
  • We still need more global planning algorithms

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases

The "compass gait" biped

The "compass gait" biped

The view from hybrid dynamics

Non-smooth mechanics

Frictional contact \(\Rightarrow\) (Measure) differential inclusions

  • Given initial conditions \(q(0), v(0)\):
    • positions \(q(t)\) absolutely continuous
    • velocities \(v(t)\) have "locally bounded variation"
  • We know how to compute analytical gradients
    • don't need neural networks for "differentiable physics"

Non-smooth mechanics

Frictional contact \(\Rightarrow\) (Measure) differential inclusions

  • Gradient-based trajectory optimization works very well! (assuming contact sequence is fixed)

 

  • "Contact-implicit trajectory optimization" tries to solve the more global problem.  Considered much harder!

OpenAI - Learning Dexterity

People like me went through a series of reactions:

  • Hmm.  I'm kind of surprised that worked.
  • Wait a minute... does that count as working?
  • If I had just run a bunch of random parameter evaluations with the same AWS budget, and kept the best one...?

Continuity of solutions w.r.t parameters

A key question for the success of gradient-based optimization

Use initial conditions here as a surrogate for dependence on policy parameters, etc.

Continuity of solutions w.r.t parameters

For the mathematical model... (ignoring numerical issues)

we do expect \(q(t_f) = F\left(q(t_0)\right)\) to be continuous.

  • Contact time, pre-/post-contact pos/vel all vary continuously.
  • Simulators will have artifacts from making discrete-time approximations; these can be made small (but often aren't)

point contact on half-plane

Continuity of solutions w.r.t parameters

We have "real" discontinuities at the corner cases

 

 

 

 

  • making contact w/ a different face
  • transitions to/from contact and no contact

Soft/compliant contact can replace discontinuities with stiff approximations

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases
  • Stochastic optimal control can smooth discontinuous objectives... but Monte-Carlo with analytical gradients of discontinuous/stiff objectives can give bad results

Non-smooth optimization

\[ \min_x f(x) \]

For gradient descent, discontinuities / non-smoothness can

  • introduce local minima
  • destroy convergence (e.g. \(l_1\)-minimization)

Smoothing discontinuous objectives

  • A natural idea: can we smooth the objective?

 

  • Probabilistic formulation, for small \(\Sigma\): \[ \min_x f(x) \approx \min_\mu E \left[ f(x) \right], x \sim \mathcal{N}(\mu, \Sigma) \]

 

  • A low-pass filter in parameter space with a Gaussian kernel.

Randomized Smoothing

J. Burke, F. E. Curtis, A. Lewis, M. Overton, and L. Simoes, Gradient Sampling Methods for Nonsmooth Optimization, 02 2020, pp. 201–225.

  • Approximate smoothed objective via Monte-carlo : \[ E_\mu \left[ f(x) \right] \approx \frac{1}{K} \sum_{i=1}^K f(x_i), \quad x_i \sim \mathcal{N}(\mu, \Sigma) \]
  • Can we also approximate the gradient? \[ \frac{\partial}{\partial \mu} E_\mu \left[ f(x) \right] \stackrel{?}{\approx} \frac{1}{K} \sum_{i=1}^K \frac{\partial f(\mu + w_i)}{\partial \mu}, \quad w_i \sim \mathcal{N}(0, \Sigma) \]

 

  • It's subtle!  Using \( \frac{f(\mu + w_i) - f(\mu)}{w_i} \) can lead to better estimates than using analytical gradients \(\frac{\partial f(\mu + w_i)}{\partial \mu} \)

Example: The Heaviside function

But \(\frac{\partial f(x)}{\partial x} = 0\) almost everywhere!  So \( \frac{1}{K} \sum_{i=1}^K \frac{\partial f(\mu + w_i)}{\partial \mu} = 0 \not\approx \frac{\partial}{\partial \mu} E_\mu [f(x)] \)

Relationship to RL Policy Gradient / CMA / MPPI

  • In reinforcement learning (RL) and "deep" model-predictive control, we add stochasticity
    • Stochastic policies
    • Random initial conditions
    • "Domain randomization"
  • then optimize a stochastic optimal control objective (e.g. maximize expected reward)

 

  • This can smooth discontinuous objectives (e.g from contact)

Relationship to RL Policy Gradient / CMA / MPPI

Big question: Why use zero-order (black-box) optimizations instead of first-order (analytical gradients)?

 

  • If we have a differentiable simulator, shouldn't we use it?
  • The Heaviside example might shed some light.

A new family of algorithms

now available on arxiv

Can we reduce sample complexity?  (with sampling strategies targeting the non-smoothness of contact?)

Relationship to other "smooth contact" models

  • Relaxed complementarity constraints from Michael Posa
  • Stochastic complementarity from Tassa and Todorov
  • ....

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases
  • Stochastic optimal control can smooth discontinuous objectives... but Monte-Carlo with analytical gradients of discontinuous/stiff objectives can give bad results
    • might explain the relative success of zero-order methods
    • new algorithms based on "randomized smoothing"
  • Beware of artificial discontinuities due to subtle interactions between the collision and physics engines
    • "Hydroelastic" contact models offer a solution

Richer contact geometries

The cartoon from walking is not rich enough for manipulation...

Rich collision geometries

My claim: Subtle interactions between the collision and physics engines can cause artificial discontinuities

(sometimes with dramatic results)

 

Understanding this requires a few steps

  1. Numerical methods must deal with overlapping geometry.
  2. Standard approaches summarize the contact forces / constraints at one or more points.
  3. It is effectively impossible to do this without introducing (potentially severe) discontinuities.

Rich collision geometries

Green arrow is the force on the red box due to the overlap with the blue box.

"Point contact" as implemented in Drake

"Point contact" as implemented in Drake

Multi-point contact

Many heuristics for using multiple points...

"Hydroelastic contact" as implemented in Drake

major contributions from Damrong Guoy, Sean Curtis, Rick Cory, Alejandro Castro, ...

"Hydroelastic contact" as implemented in Drake

Red box is rigid, blue box is soft.

"Hydroelastic contact" as implemented in Drake

Both boxes are soft.

Point contact vs hydroelastic

Point contact (discontinuous)

Hydroelastic

(continuous)

vs

Compliant ball vs. compliant box.         Rigid ball vs. compliant box                       Compliant ball vs. rigid box

Hydroelastic is

  • more expensive than point contact
  • (much) less expensive than finite-element models

 

State-space (for simulation, planning, control) is the original rigid-body state.

"Hydroelastic contact" as implemented in Drake

Point contact and multi-point contact can produce qualitatively wrong behavior.

Hydroelastic often resolves it.

Point contact vs hydroelastic

Example: Simulating LEGO® block mating

Manually-curated point contacts

Hydroelastic contact surfaces

Stable and symmetrical hydroelastic forces

Before

Now

Text

The corner cases

Point contact

Hydroelastic contact

the frictionless case

The corner cases

Point contact (no friction)

Hydroelastic

(no friction)

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases
  • Stochastic optimal control can smooth discontinuous objectives
  • ... but Monte-Carlo with analytical gradients of discontinuous/stiff objectives can give bad results
    • might explain the relative success of zero-order methods
    • new algorithms based on "randomized smoothing"
  • Beware of artificial discontinuities due to subtle interactions between the collision and physics engines
    • "Hydroelastic" contact models offer a solution
  • We still need more global planning algorithms

Smoothing isn't enough

  • Guaranteed collision-free along dynamic trajectories
  • Complete/globally optimal within convex decomposition

Shortest Paths on Graphs of Convex Sets

Tobia Marcucci, Jack Umenberger, Pablo Parrilo, Russ Tedrake. Shortest Paths in Graphs of Convex Sets. Available on arxiv.

Example: "Footstep planning" with \(x_{n+1}=Ax_n + Bu_n\)

Previous best formulations New formulation
Lower Bound
(from convex relaxation)
7% of MICP 80% of MICP

My message(s) for today

  • Contact can lead to discontinuous / stiff dependence on parameters, but primarily in the corner cases
  • Stochastic optimal control can smooth discontinuous objectives... but Monte-Carlo with analytical gradients of discontinuous/stiff objectives can give bad results
    • might explain the relative success of zero-order methods
    • new algorithms based on "randomized smoothing"
  • Beware of artificial discontinuities due to subtle interactions between the collision and physics engines
    • "Hydroelastic" contact models offer a solution
  • We still need more global planning algorithms

Commitment to Sim2Real at TRI

My MIT manipulation class is online (videos + notes)

http://manipulation.mit.edu

Simulation and Optimization with Contact

By russtedrake

Simulation and Optimization with Contact

IROS keynote, 2021

  • 1,243