Bilevel Optimization for Planning Through Contact: A Semidirect Method

Benoit Landry, Joseph Lorenzetti, Zachary Manchester, Marco Pavone

This paper presents a novel approach to one of the "boss battles" of robotics: planning through contact. Whether it's a quadruped running or a robotic hand manipulating a tool, the physics of making and breaking contact are notoriously difficult for optimization algorithms to handle.

The authors from Stanford University introduce a "semidirect" method that leverages bilevel optimization to make these problems solve faster and converge more reliably than previous state-of-the-art techniques.

TL;DR

The researchers propose a "semidirect" trajectory optimization method that splits contact forces into two parts: normal forces handled by standard constraints and friction forces solved via an embedded, differentiable quadratic program. By using bilevel optimization and analytical gradients, they achieve significantly faster solve times and better convergence on complex tasks like quadruped locomotion.

What Problem Does This Paper Address?

In robotics, "contact dynamics" are a headache. When a robot's foot hits the ground, the physics change instantly. Traditional trajectory optimization struggles with this for two reasons:

  1. Discontinuity: You are either touching the ground or you aren't. This "if-then" logic is hard for standard calculus-based optimizers.
  2. Complexity: Modeling friction usually requires "complementarity constraints" (e.g., the force is zero if the foot is in the air, but non-zero if it's touching). These constraints are numerically "brittle" and often cause solvers to crash or stall.

Existing methods usually fall into two camps: "Direct" methods (solving an optimization problem for forces at every step) or "Indirect" methods (using mathematical constraints to represent the physics). This paper argues that neither is perfect and proposes a middle ground.

Methods

The authors introduce a Semidirect Method using a Bilevel Optimization framework. Here is the breakdown:

  • The Split: They treat normal forces (the force pushing up from the ground) and friction forces (the force resisting sliding) differently.
  • Normal forces are handled "indirectly" using standard complementarity constraints.
  • Friction forces are handled "directly" as the solution to a lower-level Quadratic Program (QP) based on the "Maximum Dissipation Principle" (physics-speak for "friction tries to kill kinetic energy as fast as possible").
  • Bilevel Structure: The "Upper Level" optimizer (SNOPT) tries to find the best trajectory and control inputs. Every time it needs to know the friction forces, it calls a "Lower Level" optimizer (OSQP) to solve the friction QP.
  • Differentiable Optimization: To make this work, the upper solver needs to know how the friction forces change if it tweaks the trajectory (the gradient). The authors use the Implicit Function Theorem to calculate these gradients analytically, rather than relying on slow "black-box" approximations.
  • Parallelization: Because they use "direct transcription" (optimizing the whole path at once), they can solve the friction QPs for every single time-step in parallel, drastically speeding up the process on multi-core CPUs.

Datasets

As this is a robotics control paper, the "datasets" are simulated benchmark environments:

  1. Sliding Block: A single rigid body that must be thrown to slide and stop at a target.
  2. Hopping Robot: A 4-degree-of-freedom (DOF) leg that must jump to a specific height.
  3. Quadruped ("Little Dog"): A complex 15-DOF robot modeled after Boston Dynamics' early research platform. The task is to design a periodic walking gait.

Key Insights and Ideas

  • Decoupling is Key: By separating normal and friction forces, the authors avoid the numerical instability of putting everything into one massive, messy optimization problem. Normal forces are geometric (don't go through the floor), while friction is kinetic (resist motion).
  • Analytical Gradients Matter: Instead of letting the computer "guess" the gradients (autodiff) or using finite differences, they solve a specific linear system derived from the QP's optimality conditions. This is much more computationally efficient.
  • Exploiting Structure: Trajectory optimization often involves hundreds of "knot points" (time steps). Since the friction at time $t=5$ doesn't directly depend on the friction at $t=10$ within the lower-level solver, the bilevel approach is "embarrassingly parallel."

Key Results

  • Speedup: On the sliding block benchmark, the semidirect method was roughly 1.8x faster than the previous state-of-the-art indirect method (1.25s vs 2.24s for a 35-knot problem).
  • Reduced Problem Size: The upper-level problem for the sliding block required only 380 variables, compared to 652 for the indirect method, because the friction variables were "hidden" inside the lower-level QP.
  • Robustness: The method successfully generated a complex periodic gait for the "Little Dog" quadruped in just 1.8 seconds. The authors noted that the previous state-of-the-art method failed to converge to a good solution for this specific task.

Limitations and Future Work

  • Subgradient Selection: When the friction problem doesn't have a unique solution (e.g., when the robot is perfectly still), the gradient isn't well-defined. The authors used a "least squares" heuristic to pick a gradient, but they admit this lacks a rigorous theoretical foundation.
  • Sticking Friction: The upper-level solver (SNOPT) still struggled occasionally with "sticking" contact (where velocity is exactly zero), likely due to sharp changes in the gradients of the lower-level problem.
  • Future Directions: The authors suggest investigating better ways to handle non-unique gradients and exploring how this method could be used for real-time model predictive control (MPC).

Summary updated

Abstract

Many robotics applications, from object manipulation to locomotion, require planning methods that are capable of handling the dynamics of contact. Trajectory optimization has been shown to be a viable approach that can be made to support contact dynamics. However, the current state-of-the art methods remain slow and are often difficult to get to converge. In this work, we leverage recent advances in bilevel optimization to design an algorithm capable of efficiently generating trajectories that involve making and breaking contact. We demonstrate our method’s efficiency by outperforming an alternative state-of-the-art method on two benchmark problems. We moreover demonstrate the method’s ability to design a simple periodic gait for a quadruped with 15 degrees of freedom and four contact points.
BibTeX
@inproceedings{landry2019bilevel,
  author = {Benoit Landry and Joseph Lorenzetti and Zachary Manchester and Marco Pavone},
  title = {{Bilevel Optimization for Planning Through Contact: A Semidirect Method}},
  booktitle = {ISRR},
  year = {2019},
  url = {https://openreview.net/forum?id=0OeGyVhbdXS},
}

Loading related content...

Figure