Sampling-Based Planning for Retrieving Near-Cylindrical Objects in Cluttered Scenes Using Hierarchical Graphs

We present an incremental sampling-based task and motion planner for retrieving near-cylindrical objects, like bottle, in cluttered scenes, which computes a plan for removing obstacles to generate a collision-free motion of a robot to retrieve the target object. Our proposed planner uses a two-level hierarchy, including the first-level roadmap for the target object motion and the second-level retrieval graph for the entire robot motion, to aid in deciding the order and trajectory of object removal. We use an incremental expansion strategy to update the roadmap and retrieval graph from the collisions between the target object, the robot, and the obstacles, in order to optimize the object removal sequence. The performance of our method is highlighted in several benchmark scenes, including a fixed robotic arm in a cluttered scene with known obstacle locations and a scene, where locations of some objects or even the target object are unknown due to occlusions. Our method can also efficiently solve the high-dimensional planning problem of object retrieval using a mobile manipulator and be combined with the symbolic planner to plan complex multistep tasks. We deploy our method to a physical robot and integrate it with nonprehensile actions to improve operational efficiency. Compared to the state-of-the-art approaches, our method reduces task and motion planning time up to 24.6\% with a higher success rate, and still provides a near-optimal plan.