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

TL;DR

This paper addresses optimal task and motion planning for high-DoF mobile manipulators under geometric constraints. It introduces Reachability-guided LGP (R-LGP), a three-level optimization framework that integrates a LazyPRM-inspired reachability graph with logic-geometric programming to guide symbolic planning and the final trajectory optimization. Key contributions include a reachability graph that validates end-effector poses in $x \in \mathbb{R}^3$ and configurations $q \in \mathbb{R}^{3+m}$, a solution library for fast path reuse, and a complete TAMP pipeline that achieves higher success rates and lower planning times compared to baselines. Experiments on simulation and real hardware (Toyota HSR) demonstrate improved efficiency and collision-free, near-optimal trajectories, indicating practical viability for complex mobile-manipulator tasks.

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.

Figures (7)

• Figure 1: Physical HSR performing R-LGP solution for table clearing task. The captured actions include grabbing knob, opening/closing drawer, picking object, and dropping object.
• Figure 2: Path querying system in R-LGP.
• Figure 3: Comparison result of the TAMP planners on pick and place task.
• Figure 4: Comparison result of the TAMP planners on sorting task.
• Figure 5: TAMP results. Objects are initially spawned on the grey table and should be put on the table with the same color. R-LGP outperforms RHH-LGP in generating reachability-aware robot base trajectory (yellow lines) and avoiding collisions (red lines).