Symbolic State Space Optimization for Long Horizon Mobile Manipulation Planning

[Paper]  [Poster]  [Code coming soon...]

Accepted to IROS 2023

Xiaohan Zhang, Yifeng Zhu, Yan Ding, Yuqian Jiang, Yuke Zhu, Peter Stone, Shiqi Zhang

IROS23_2217.mp4

Abstract

In existing task and motion planning (TAMP) research, it is a common assumption that experts manually specify the state space for task-level planning. A well-developed state space enables the desirable distribution of limited computational resources between task planning and motion planning. However, developing such task-level state spaces can be non-trivial in practice. In this paper, we consider a long horizon mobile manipulation domain including repeated navigation and manipulation. We propose Symbolic State Space Optimization (S3O) for computing a set of abstracted locations and their 2D geometric groundings for generating task-motion plans in such domains. Our approach has been extensively evaluated in simulation and demonstrated on a real mobile manipulator working on clearing up dining tables. Results show the superiority of the proposed method over TAMP baselines in task completion rate and execution time.

Overview

Objects are frequently in separate symbolic locations in a predefined task planner. A TAMP system with such a fine-grained state space would always generate plans that suggest the robot navigate before every manipulation. However, if an optimized state space can include multiple objects (that are close to each other) in a single location, the robot will be able to navigate once and perform a sequence of manipulation actions. We aim to answer how to compute such symbolic locations and their geometric groundings.

Scoring Function for State Space Ranking


Left Figure: Action feasibility values are computed using robot perception and represented as heatmaps. 

Right Figure: A top-ranked Voronoi Partition for the state space generated using S3O, where objects A and B are in one symbolic location, and objects E and F are in another one.

CMA-ES for Efficient Motion-level Search

Samples (cyan pixels) drawn from the CMA-ES sampler at early and late iterations. With action feasibility and efficiency being considered in the objective function, robot base positions gradually converge to a sequence of areas that are close to the objects for the robot to reach, and are of a low overall navigation cost.

Quantitative Evalution  (Simulation)

Overall performances of our approach and four baseline methods in task completion rate and robot execution time (s). Tasks are grouped based on their difficulties. S3O produced the highest task completion rate while maintaining the lowest robot execution time. This observation is consistent over tasks of different difficulties.

Real Robot Demonstration -- Supplementary Video

s3o_demo.mp4