Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

D-LGP: Dynamic Logic-Geometric Program for Reactive Task and Motion Planning

Teng Xue, Amirreza Razmjoo, Sylvain Calinon

TL;DR

This work tackles long-horizon task-and-motion planning (TAMP) by introducing Dynamic Tree Search (DTS) for target-directed, horizon-free task skeletons and Dynamic Logic-Geometric Programming (D-LGP) that couples DTS with global optimization via mixed-integer convex programming. DTS performs backward dynamic programming from a known target configuration to prune infeasible branches, while motion planning is reformulated as a MIQP by decomposing the non-convex space into convex subspaces and encoding choices with binary indicators $z_i$ in a big-$M$ framework. The approach is validated on three tabletop benchmarks and real-robot experiments, demonstrating faster, more reliable planning and robust reactivity to disturbances at about 10 Hz, with the ability to replan from updated camera feedback. The results suggest that fast, closed-loop TAMP can be achieved without sacrificing optimality and point to avenues for scalability and integration with learning-based planning.

Abstract

Many real-world sequential manipulation tasks involve a combination of discrete symbolic search and continuous motion planning, collectively known as combined task and motion planning (TAMP). However, prevailing methods often struggle with the computational burden and intricate combinatorial challenges, limiting their applications for online replanning in the real world. To address this, we propose Dynamic Logic-Geometric Program (D-LGP), a novel approach integrating Dynamic Tree Search and global optimization for efficient hybrid planning. Through empirical evaluation on three benchmarks, we demonstrate the efficacy of our approach, showcasing superior performance in comparison to state-of-the-art techniques. We validate our approach through simulation and demonstrate its reactive capability to cope with online uncertainty and external disturbances in the real world. Project webpage: https://sites.google.com/view/dyn-lgp.

D-LGP: Dynamic Logic-Geometric Program for Reactive Task and Motion Planning

TL;DR

This work tackles long-horizon task-and-motion planning (TAMP) by introducing Dynamic Tree Search (DTS) for target-directed, horizon-free task skeletons and Dynamic Logic-Geometric Programming (D-LGP) that couples DTS with global optimization via mixed-integer convex programming. DTS performs backward dynamic programming from a known target configuration to prune infeasible branches, while motion planning is reformulated as a MIQP by decomposing the non-convex space into convex subspaces and encoding choices with binary indicators in a big- framework. The approach is validated on three tabletop benchmarks and real-robot experiments, demonstrating faster, more reliable planning and robust reactivity to disturbances at about 10 Hz, with the ability to replan from updated camera feedback. The results suggest that fast, closed-loop TAMP can be achieved without sacrificing optimality and point to avenues for scalability and integration with learning-based planning.

Abstract

Many real-world sequential manipulation tasks involve a combination of discrete symbolic search and continuous motion planning, collectively known as combined task and motion planning (TAMP). However, prevailing methods often struggle with the computational burden and intricate combinatorial challenges, limiting their applications for online replanning in the real world. To address this, we propose Dynamic Logic-Geometric Program (D-LGP), a novel approach integrating Dynamic Tree Search and global optimization for efficient hybrid planning. Through empirical evaluation on three benchmarks, we demonstrate the efficacy of our approach, showcasing superior performance in comparison to state-of-the-art techniques. We validate our approach through simulation and demonstrate its reactive capability to cope with online uncertainty and external disturbances in the real world. Project webpage: https://sites.google.com/view/dyn-lgp.
Paper Structure (13 sections, 5 equations, 3 figures, 3 tables, 2 algorithms)

This paper contains 13 sections, 5 equations, 3 figures, 3 tables, 2 algorithms.

Table of Contents

  1. Introduction
  2. Related Work
  3. Method
  4. Target-centric LGP
  5. Mixed-integer Motion Planning
  6. D-LGP Algorithm
  7. Experiments
  8. Benchmarks
  9. DTS vs MBTS
  10. MIP vs SLSQP and IPOPT
  11. D-LGP Performance
  12. Real robot experiments
  13. Conclusion and Future Work

Figures (3)

  • Figure 1: System setup where the robot constructs a tower sequentially. The objective is to stack the blocks at a specified target point in a predefined order. If a block is out of reach (indicated by the red curve), the robot must determine how to use a tool to pull the block into the reachable region before continuing the pick-and-place operation.
  • Figure 2: Illustrative example of Dynamic Tree Search. Given the initial and target configurations, $succ^\dagger(s_0, s_{K})$ will infer the essential action as Pick[C #P3] (shown as dotted line in Fig. \ref{['block:solution']}). However, this is infeasible because B is on top of C. $sub\_{goal}(\cdot)$ will identify the essential subgoal as moving B to #p2 (given by MIQP) first. Then, C can be picked and placed at #p3, followed by picking B and A until reaching the final target.
  • Figure 3: Tower construction task with tool usage. The configuration is initialized as (a) and the objective is to stack blocks as (g). The first step is to pick and place D at the target point (b). Then, an external disturbance occurs, moving block C out of reachability. To resolve this, the robot uses a tool (c) to pull block C into the reachability region (d). Once done, the robot can pick C as usual and place it on top of D (c). Next, B (f) and A (g) are placed in sequence.