Introduction
Planning through contact remains one of the most significant challenges in robotics. Whether a quadruped is navigating uneven terrain or a robotic hand is manipulating a tool, the physics of making and breaking contact introduce discontinuities that are notoriously difficult for standard optimization algorithms. Traditional trajectory optimization methods often struggle with the "if-then" logic of contact—where forces are zero when in the air and non-zero when touching—leading to numerical instability and slow convergence.
In this work, researchers from Stanford University propose a "semidirect" method that leverages bilevel optimization to address these challenges. By decoupling normal and friction forces and utilizing analytical gradients, the method achieves superior performance and robustness compared to existing state-of-the-art techniques.
The Semidirect Approach
The core innovation of this paper is the division of contact forces into two distinct categories handled by a bilevel optimization framework.
Decoupling Forces
Existing methods typically treat all contact forces either "directly" (as decision variables) or "indirectly" (through mathematical constraints). This paper argues for a hybrid approach:
- Normal Forces: These are handled "indirectly" using standard complementarity constraints. This ensures the robot does not penetrate the ground while allowing for the making and breaking of contact.
- Friction Forces: These are handled "directly" as the solution to a lower-level Quadratic Program (QP). This QP is based on the "Maximum Dissipation Principle," which dictates that friction acts to minimize kinetic energy.
Bilevel Structure
This split creates a bilevel structure. An upper-level optimizer (SNOPT) searches for the optimal trajectory and control inputs. Whenever the upper-level solver needs to evaluate friction forces, it calls a lower-level optimizer (OSQP) to solve the friction QP.
To ensure the upper-level solver can navigate the search space effectively, it requires the gradients of the friction forces with respect to the trajectory. The authors utilize the Implicit Function Theorem to calculate these analytical gradients. This approach is significantly more efficient than black-box approximations or finite differences, providing the exact sensitivity of the friction solution to changes in the robot's state.
Computational Efficiency and Parallelization
The semidirect method offers several computational advantages over traditional approaches:
- Reduced Problem Size: By embedding friction forces within a lower-level QP, the number of decision variables in the upper-level problem is significantly reduced. For a sliding block benchmark, the upper-level variables dropped from 652 to 380.
- Parallelization: The authors use direct transcription, which discretizes the trajectory into "knot points." Because the friction QP at any given time step is independent of others within the lower-level solver, these problems can be solved in parallel across multiple CPU cores, drastically reducing total computation time.
Experimental Results
The method was evaluated on three simulated benchmarks: a sliding block, a hopping robot, and a 15-degree-of-freedom (DOF) quadruped ("Little Dog").
Performance Benchmarks
On the sliding block task, the semidirect method demonstrated a 1.8x speedup over the previous state-of-the-art indirect method, solving a 35-knot problem in 1.25 seconds compared to 2.24 seconds.
Complex Locomotion
The most impressive demonstration involved the "Little Dog" quadruped. The task was to design a periodic walking gait for the 15-DOF system with four contact points. The semidirect method generated a stable gait in just 1.8 seconds. Notably, the authors reported that the alternative state-of-the-art method failed to converge to a viable solution for this complex task, highlighting the robustness and reliability of the bilevel approach.
Limitations and Future Directions
While the semidirect method represents a significant step forward, some challenges remain. A primary limitation is the handling of non-unique gradients. In scenarios where the friction problem does not have a unique solution (such as when the robot is stationary), the gradient is not well-defined. The authors currently use a least-squares heuristic to select a subgradient, but a more rigorous theoretical treatment is needed.
Additionally, the solver occasionally struggles with "sticking" friction, where velocities are exactly zero. Future work will focus on refining these edge cases and exploring the application of this method to real-time Model Predictive Control (MPC), where the speed and reliability of the semidirect method could enable more dynamic, reactive robot behaviors in the real world.
Summary updated