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

We devise a hierarchical planner that integrates contact-aware trajectory optimization, collision-free motion planning, and MIP-based graph search over reachable set primitives. Our approach enables a bimanual KUKA system to reorient large, unwieldy objects using whole-arm contact in under a minute, achieving >60% lower cost plans than a leading sampling-based method.