Introduction
Combined Task and Motion Planning (TAMP) is a fundamental challenge in robotics, requiring systems to bridge the gap between high-level symbolic reasoning (e.g., "pick up block A") and low-level geometric execution (e.g., "move the arm along this path"). Traditionally, these layers are decoupled: a symbolic planner generates a sequence of actions, and a geometric planner subsequently checks for physical feasibility.
However, this separation often fails when tasks involve continuous geometric objectives, such as maximizing the stability or height of a structure. Standard symbolic planners cannot "see" these geometric costs, making it difficult to find truly optimal solutions for complex, multi-step manipulation tasks. This paper introduces Logic-Geometric Programming (LGP), a framework that treats TAMP as a single, unified optimization problem. By using logic to structure geometric constraints rather than just checking for feasibility, LGP can solve complex manipulation tasks defined by continuous objectives.
The LGP Framework
The core contribution is the formulation of TAMP as a Logic-Geometric Program—a non-linear constrained optimization problem where the symbolic action sequence determines which geometric constraints are active at different points in time. Instead of abstracting geometry into symbolic predicates, LGP "pushes" logic into the mathematical program. Logic serves as a "switchboard" that activates or deactivates geometric constraints based on the chosen sequence of actions.
A key concept in this framework is the treatment of kinematic switches. Grasping and releasing objects are modeled not just as discrete events, but as fundamental changes in the world's kinematic tree. For instance, when a robot grasps a block, that block becomes a "child" of the robot's end-effector in the optimization's coordinate system, altering the underlying degrees of freedom and constraints.
Hierarchical Optimization
To manage the computational complexity of solving such a high-dimensional, non-linear problem, the authors propose a three-level hierarchical approach:
- Level 1: Effective End Space Optimization: This fast heuristic optimizes only the final configuration of all objects. It evaluates whether a given sequence of moves could potentially result in a high-quality, stable final state, allowing the search to prune infeasible symbolic plans in milliseconds.
- Level 2: Keyframe Optimization: This level optimizes the "snapshots" of the robot and objects at critical interaction points (the "switches"), such as the exact moments of grasping or placement.
- Level 3: Full Path Optimization: Utilizing k-order motion optimization (KOMO), this level solves for the complete, smooth trajectory of the robot and all objects, ensuring collision avoidance and minimizing control costs.
High-level search over action sequences is conducted using Monte Carlo Tree Search (MCTS), guided by the Level 1 optimization as a powerful heuristic.
Experimental Results
The authors evaluated LGP in a custom physics-based simulation where a robot arm was tasked with building structures from various boards, cylinders, and blocks. The primary objective was to maximize the height of a specific object while maintaining physical stability.
The system successfully generated plans for up to 50 sequential manipulations, a depth that typically causes traditional TAMP planners to struggle due to the cumulative cost of geometric feasibility checks.
As shown in the performance plots, the Level 1 (End Space) optimization is remarkably efficient, taking less than one second even for scenes containing 100 objects. This efficiency enables real-time symbolic search. Qualitatively, the system does not merely find a feasible stack; it optimizes for stability by placing blocks centrally and spreading supports, as the geometric objective is integrated directly into the search process.
Conclusion and Future Work
LGP demonstrates that integrating logic and geometry into a single optimization framework allows for solving highly complex manipulation tasks that were previously out of reach. While the system handles geometry efficiently, future work is needed to improve the symbolic search's performance over very deep logical horizons. Transitioning these results from simulation to high-DoF real-world robots remains a significant next step.
Summary updated