Hierarchical Policy Blending As Optimal Transport

An T. Le, Kay Hansel, Jan Peters, Georgia Chalvatzaki

Introduction

Robotic systems operating in dynamic real-world environments face a fundamental tension between reactivity and planning. Reactive policies, such as Riemannian Motion Policies (RMPs), provide high-frequency obstacle avoidance but often suffer from "myopia," becoming trapped in local minima like U-shaped obstacles. Conversely, global planners offer foresight but are computationally intensive, often failing to meet the real-time constraints required for fluid, safe motion.

HiPBOT (Hierarchical Policy Blending as Optimal Transport) bridges this gap. By formulating policy blending as a convex optimization problem using Unbalanced Optimal Transport (UOT), HiPBOT dynamically weights reactive "expert" policies. This approach combines the immediate safety of reactive control with the strategic foresight of look-ahead planning, significantly outperforming traditional inference-based methods in both speed and success rate.

Methodology: Blending via Optimal Transport

HiPBOT structures robot control into a two-level hierarchy:

  1. Low-Level (Reactive Experts): The system utilizes RMPs, which represent modular behaviors as a "Product of Experts" (PoE). These experts handle immediate tasks like goal reaching and collision avoidance.
  2. High-Level (The Blender): This layer determines the optimal weights ($\beta$) for each expert. Instead of static weighting, HiPBOT employs Entropic-Regularized Unbalanced Optimal Transport (UOT) to decide how much to trust each expert at any given moment.

Why Unbalanced Optimal Transport?

Standard Optimal Transport requires mass conservation, meaning weights must sum to one. In robotics, the "importance" of control effort is not constant; for instance, an imminent collision requires a surge in avoidance weight that may exceed the total "mass" of a standard goal-seeking state. UOT allows for "mass creation or destruction," providing a mathematically rigorous way to handle unnormalized policy weights. By applying the Sinkhorn-Knopp algorithm, HiPBOT solves this optimization rapidly, enabling the robot to plan in the lower-dimensional parameter space of its policies rather than the high-dimensional physical coordinate space.

Trajectory planning in 2D environments: Planar Maze (left) and Planar Box (right), showing the path from start (red) to goal (green).

Experimental Evaluation

The framework was evaluated across several challenging scenarios:

  • 2D Toy Environments: Including a cluttered "Planar Maze" with dynamic obstacles and a "Planar Box" designed to induce local minima.
  • 7DoF Panda Manipulator: A high-dimensional task requiring a robot arm to reach targets while navigating around static and moving obstacles.
  • TIAGo++ Whole-Body Control: A complex multi-agent scenario involving coordinated arm movements and self-collision avoidance.

Key Results and Insights

HiPBOT demonstrates superior performance in both safety and efficiency:

  • Success Rates: In the 2D Maze, HiPBOT achieved an 82% success rate, surpassing the 73% baseline. In the "Box" environment, it reached a 93% success rate, where standard RMPflow baselines failed entirely.
  • Computational Efficiency: Thanks to the parallelizable nature of the Sinkhorn algorithm, HiPBOT achieves planning rates of 30Hz with a 10-step look-ahead. In contrast, the state-of-the-art HiPBI baseline requires a 50-step look-ahead and runs at only 2Hz to achieve similar results.
  • Stability: The authors provide proofs for asymptotic local stability in static environments when using RMPs as underlying experts.
The 7DoF Panda Manipulator task. HiPBOT dynamically weights avoidance and reaching experts to navigate around red spherical obstacles toward the target.

Conclusion

HiPBOT introduces a novel paradigm for robot control by treating policy blending as a resource allocation problem solved via Optimal Transport. This formulation allows for high-speed, globally-aware reactive control. Future work aims to extend stability guarantees to highly dynamic environments and integrate HiPBOT with symbolic high-level task planners to handle complex multi-stage missions.

Summary updated

BibTeX
@inproceedings{le2023hierarchical,
  author = {An T. Le and Kay Hansel and Jan Peters and Georgia Chalvatzaki},
  title = {{Hierarchical Policy Blending As Optimal Transport}},
  booktitle = {L4DC},
  year = {2023},
  url = {https://openreview.net/forum?id=uQMNocE6FW},
}
Figure