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.
Introduction
Task and Motion Planning (TAMP) requires robots to bridge the gap between high-level symbolic logic (e.g., "pick up the apple") and low-level physical execution (e.g., joint trajectories). A critical component of this process is the "symbolic state space"—the set of locations and labels the robot uses to reason about its environment. Traditionally, these spaces are manually defined by human experts.
However, manual definitions often lead to sub-optimal performance. If an expert defines a unique location for every object, the high-level planner assumes it must navigate to a new coordinate for every item, resulting in redundant movements. Conversely, if locations are too broad, the robot may attempt actions from positions where targets are physically unreachable. S3O addresses this by automatically finding a "Goldilocks" zone of symbolic locations that are both efficient and physically feasible.
Methodology
The S3O framework optimizes the symbolic state space through a hierarchical three-step process designed to balance navigation efficiency with manipulation feasibility.
1. Candidate Generation via Voronoi Partitioning
The system begins by using the 2D positions of objects to create a Voronoi diagram, partitioning the workspace into discrete regions. It then generates various candidate state spaces by merging adjacent regions. This allows the system to evaluate different ways of grouping objects (e.g., treating "Can A" and "Can B" as being within the same symbolic location).
2. Feasibility-Based Ranking
Candidate spaces are ranked using a scoring function based on Action Feasibility. S3O utilizes a learned Fully Convolutional Network (FCN-VGG16) to generate feasibility heatmaps. These heatmaps predict the probability of a successful pick-up from any given 2D coordinate, accounting for both the robot's reach and unmapped obstacles like chairs or table legs.
3. Motion-Level Optimization
Once a symbolic plan is selected (e.g., "Go to Location 1, pick A, pick B"), the system employs Covariance Matrix Adaptation Evolution Strategy (CMA-ES) to find the optimal 2D coordinates within those regions. This ensures the robot stands in a spot that minimizes travel time while maximizing the reachability of all targets in that group.
Experimental Evaluation
The feasibility model (FCN) was trained on a dataset of robot experiences consisting of grayscale heatmap images representing successful and failed navigation/manipulation attempts. Evaluation was conducted in a simulated "restaurant" environment using the Gazebo physics engine, featuring seven tables and various randomly placed obstacles. Finally, the system was validated on a Toyota Human Support Robot (HSR) performing a real-world "tidy home" task.
Key Results and Discussion
The proposed S3O method demonstrated significant improvements over standard TAMP baselines:
- Higher Completion Rates: In "Difficult" scenarios with dense obstacles, S3O-GROP* achieved a task completion rate of ~0.29, compared to ~0.21 for the standard V-GROP baseline.
- Reduced Execution Time: Optimized state spaces led to faster task completion across all difficulty levels. In "Easy" tasks, S3O reduced execution time to ~108 seconds, a 21% improvement over the ~138 seconds required by non-optimized baselines.
- Ablation Insights: Comparisons with a random state space selection (S3O-Random) showed that while random grouping can be fast, it often results in unfeasible plans. This confirms that the ranking mechanism is vital for selecting locations that are both efficient and reachable.
Conclusion and Future Work
S3O successfully automates the creation of high-level symbolic locations, allowing robots to treat clusters of objects as single task-level stops. This approach mirrors human-like efficiency in tasks such as clearing a table. Future work will focus on extending the perception-based feasibility model from 2D top-down views to 3D voxel-based or first-person perception to handle more complex environments like shelving. Additionally, the authors aim to incorporate dynamic obstacle avoidance into the state space optimization phase.
Summary updated