R-LGP: A Reachability-guided Logic-geometric Programming Framework for Optimal Task and Motion Planning on Mobile Manipulators

TL;DR

This paper introduces R-LGP, a reachability-guided logic-geometric programming framework that enhances task and motion planning for high-DoF mobile manipulators by incorporating environmental constraints and improving efficiency.

Contribution

The work extends LGP with a sampling-based reachability graph to better handle high-dimensional systems and obstacle avoidance, reducing replanning and increasing success rates.

Findings

Outperforms state-of-the-art in success rate and planning time

Proves effective on physical Toyota HSR robot

Achieves collision-free, optimal solutions efficiently

Abstract

This paper presents an optimization-based solution to task and motion planning (TAMP) on mobile manipulators. Logic-geometric programming (LGP) has shown promising capabilities for optimally dealing with hybrid TAMP problems that involve abstract and geometric constraints. However, LGP does not scale well to high-dimensional systems (e.g. mobile manipulators) and can suffer from obstacle avoidance issues due to local minima. In this work, we extend LGP with a sampling-based reachability graph to enable solving optimal TAMP on high-DoF mobile manipulators. The proposed reachability graph can incorporate environmental information (obstacles) to provide the planner with sufficient geometric constraints. This reachability-aware heuristic efficiently prunes infeasible sequences of actions in the continuous domain, hence, it reduces replanning by securing feasibility at the final full path trajectory optimization. Our framework proves to be time-efficient in computing optimal and collision-free solutions, while outperforming the current state of the art on metrics of success rate, planning time, path length and number of steps. We validate our framework on the physical Toyota HSR robot and report comparisons on a series of mobile manipulation tasks of increasing difficulty. Videos of the experiments are available at https://youtu.be/NEVVHEhQnOQ.