A Hierarchical Framework for Long Horizon Planning of Object-Contact Trajectories

Given an object, an environment, and a goal pose, how should a robot make contact to move it? Solving this problem requires reasoning about rigid-body dynamics, object and environment geometries, and hybrid contact mechanics. This paper proposes a hierarchical framework that solves this problem in 2D worlds, with polygonal objects and point fingers. To achieve this, we decouple the problem in three stages: 1) a high-level graph search over regions of free-space, 2) a medium-level randomized motion planner for the object motion, and 3) a low-level contact-trajectory optimization for the robot and environment contacts. In contrast to the state of the art, this approach does not rely on handcrafted primitives and can still be solved efficiently. This algorithm does not require seeding and can be applied to complex object shapes and environments. We validate this framework with extensive simulated experiments showcasing long-horizon and contact-rich interactions. We demonstrate how our algorithm can reliably solve complex planar manipulation problems in the order of seconds.