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

TL;DR

The paper addresses the difficulty of globally optimizing contact-rich manipulation by introducing an object-centric discrete decision space built from mutual reachable sets (MRS) and an offline graph of convex approximations. An online hierarchical planner then uses Graph of Convex Sets (GCS) to obtain approximately optimal object-space trajectories and translates them into full configuration-space control sequences via CQDC-MPC and collision-free planning. The approach achieves significant improvements over a state-of-the-art baseline in path quality, success rate, and query time, demonstrated on a challenging SE(2) CRM task with real hardware validation. This work enables practical, near-optimal CRM planning and charts a path toward scalable, reachability-informed planning in more complex spaces like SE(3).

Abstract

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.

Figures (14)

• Figure 1: The bimanual KUKA iiwa-7 hardware setup with the cylindrical object. Because both the manipulators and the object are constrained to the $xy$-plane, only the three indicated joints are actuated on each arm. The object pose (shown) is defined with respect to the world frame located at the bimanual base (also shown).
• Figure 2: An illustration of different configurations ($q_{\text{seed}}$, $q_1$, $q_2$, $q_3$) inside an example MRS. Note how this single MRS encapsulates different contact modes (in $q_{\text{seed}}$, both wrists make contact; in $q_2$, a white end-effector makes contact; etc.). This is what makes MRS a useful discrete decision space - it abstracts away some of the combinatorial complexity of contact modes.
• Figure 3: This figure shows the relationship between FRS $\mathcal{R}^{+} \in \mathcal{Q}$, BRS $\mathcal{R}^{-} \in \mathcal{Q}$, and MRS $\mathcal{R}^{o} \in \mathcal{Q}^{o}$. The FRS and BRS lie on contact manifolds in the full configuration space. They are defined by seed configuration $q_{seed}$, where they intersect, and the choice of contact-aware trajectory optimizer $\pi$. The MRS is defined in lower-dimensional object space, as the intersection of the projections of the FRS and BRS. We also illustrate the inverse projection operator, $\mathbf{proj}_{\mathcal{Q}^{o}}^{-1}$, that maps object states in the MRS to their full configurations.
• Figure 4: Left: visual geometry of the bimanual KUKA iiwa-7 system, with the object workspace $\mathcal{Q}^{o}$ overlaid in green. $\mathcal{Q}^{o}$ describes all object positions that can be grasped. Right: collision model used for planning. Each arm has 14 spheres.
• Figure 5: Demonstration of computing a convex-approximated MRS on a toy example in 2D object space. Left: after we compute the projected FRS $\mathcal{R}^{o, +}_{\Delta}$ (orange) and BRS $\mathcal{R}^{o, -}_{\Delta}$ (green), we intersect them to get the discrete MRS $\mathcal{R}^{o}_{\Delta}$ (brown). Right: next, we find a convex approximation of $\mathcal{R}^{o}_{\Delta}$. The figure shows a likely convex approximation produced by the algorithm IRIS-ZO: $\hat{\mathcal{R}}^{o}_{\Delta}$ (violet). It lies mostly within $\mathcal{R}^{o}_{\Delta}$, but may exceed it somewhat. We want to avoid such "overapproximation" as much as possible, since it amounts to introducing unreachable sets into our reachable set approximation.

Theorems & Definitions (6)

• Definition 1
• Lemma 1
• Definition 2
• Lemma 1
• proof
• proof