# Approximately Optimal Global Planning for Contact-Rich SE(2) Manipulation on a Graph of Reachable Sets

arXiv: https://arxiv.org/abs/2601.10827 · HTML: https://mlsanity.com/paper/2601.10827/

## Key facts

- No citing papers in the map yet (data through 14 Oct 2026).
- Builds on 35 of its 54 references that are in the map.
- First posted on arXiv 15 Jan 2026.

## Builds on (most cited first, 30 of 35)

- [Diffusion Policy: Visuomotor Policy Learning via Action Diffusion](https://mlsanity.com/paper/2303.04137/) (2023)
- [Convex optimization](https://mlsanity.com/paper/2106.01946/) (2021)
- [π0: A Vision-Language-Action Flow Model for General Robot Control](https://mlsanity.com/paper/2410.24164/) (2024)
- [Octo: An Open-Source Generalist Robot Policy](https://mlsanity.com/paper/2405.12213/) (2024)
- [Planning with Diffusion for Flexible Behavior Synthesis](https://mlsanity.com/paper/2205.09991/) (2022)
- [QT-Opt: Scalable Deep Reinforcement Learning for Vision-Based Robotic Manipulation](https://mlsanity.com/paper/1806.10293/) (2018)
- [Learning Dexterous In-Hand Manipulation](https://mlsanity.com/paper/1808.00177/) (2018)
- [A System for General In-Hand Object Re-Orientation](https://mlsanity.com/paper/2111.03043/) (2021)
- [DeXtreme: Transfer of Agile In-hand Manipulation from Simulation to Reality](https://mlsanity.com/paper/2210.13702/) (2022)
- [ALOHA Unleashed: A Simple Recipe for Robot Dexterity](https://mlsanity.com/paper/2410.13126/) (2024)
- [A Careful Examination of Large Behavior Models for Multitask Dexterous Manipulation](https://mlsanity.com/paper/2507.05331/) (2025)
- [On Bringing Robots Home](https://mlsanity.com/paper/2311.16098/) (2023)
- [Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-dynamic Contact Models](https://mlsanity.com/paper/2206.10787/) (2022)
- [Predictive Sampling: Real-time Behaviour Synthesis with MuJoCo](https://mlsanity.com/paper/2212.00541/) (2022)
- [HACMan: Learning Hybrid Actor-Critic Maps for 6D Non-Prehensile Manipulation](https://mlsanity.com/paper/2305.03942/) (2023)
- [Motion Planning around Obstacles with Convex Optimization](https://mlsanity.com/paper/2205.04422/) (2022)
- [Dojo: A Differentiable Physics Engine for Robotics](https://mlsanity.com/paper/2203.00806/) (2022)
- [Shortest Paths in Graphs of Convex Sets](https://mlsanity.com/paper/2101.11565/) (2021)
- [Contact Mode Guided Motion Planning for Quasidynamic Dexterous Manipulation in 3D](https://mlsanity.com/paper/2105.14431/) (2021)
- [DROP: Dexterous Reorientation via Online Planning](https://mlsanity.com/paper/2409.14562/) (2024)
- [Contact Mode Guided Sampling-Based Planning for Quasistatic Dexterous Manipulation in 2D](https://mlsanity.com/paper/2011.01454/) (2020)
- [Enhancing Dexterity in Robotic Manipulation via Hierarchical Contact Exploration](https://mlsanity.com/paper/2307.00383/) (2023)
- [In-Hand Manipulation via Motion Cones](https://mlsanity.com/paper/1810.00219/) (2018)
- [Contact-Implicit Trajectory Optimization for Dynamic Object Manipulation](https://mlsanity.com/paper/2103.01104/) (2021)
- [Towards Tight Convex Relaxations for Contact-Rich Manipulation](https://mlsanity.com/paper/2402.10312/) (2024)
- [Dexterous Contact-Rich Manipulation via the Contact Trust Region](https://mlsanity.com/paper/2505.02291/) (2025)
- [Inverse Dynamics Trajectory Optimization for Contact-Implicit Model Predictive Control](https://mlsanity.com/paper/2309.01813/) (2023)
- [Real-Time Multi-Contact Model Predictive Control via ADMM](https://mlsanity.com/paper/2109.07076/) (2021)
- [Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets](https://mlsanity.com/paper/2305.06341/) (2023)
- [TrajectoTree: Trajectory Optimization Meets Tree Search for Planning Multi-contact Dexterous Manipulation](https://mlsanity.com/paper/2109.14088/) (2021)

## Abstract (arXiv)

If we consider human manipulation, it is clear that contact-rich manipulation (CRM)-the ability to use any surface of the manipulator to make contact with objects-can be far more efficient and natural than relying solely on end-effectors (i.e., fingertips). However, state-of-the-art model-based planners for CRM are still focused on feasibility rather than optimality, limiting their ability to fully exploit CRM's advantages. We introduce a new paradigm that computes approximately optimal manipulator plans. This approach has two phases. Offline, we construct a graph of mutual reachable sets, where each set contains all object orientations reachable from a starting object orientation and grasp. Online, we plan over this graph, effectively computing and sequencing local plans for globally optimized motion. On a challenging, representative contact-rich task, our approach outperforms a leading planner, reducing task cost by 61%. It also achieves a 91% success rate across 250 queries and maintains sub-minute query times, ultimately demonstrating that globally optimized contact-rich manipulation is now practical for real-world tasks.