Abstract
We address the problem of applying Task and Motion Planning (TAMP) in real-world environments. TAMP combines symbolic and geometric reasoning to produce sequential manipulation plans, typically specified as joint-space trajectories, which are valid only as long as the environment is static and perception and control are highly accurate. In case of any changes in the environment, slow re-planning is required. We propose a TAMP algorithm that optimizes over Cartesian frames defined relative to target objects. The resulting plan then remains valid even if the objects are moving and can be executed by reactive controllers that adapt to these changes in real time. We apply our TAMP framework to a torque-controlled robot in a pick and place setting and demonstrate its ability to adapt to changing environments, inaccurate perception, and imprecise control, both in simulation and the real world.
Introduction
Task and Motion Planning (TAMP) serves as the cognitive architecture for complex robotic manipulation, bridging the gap between high-level symbolic logic (e.g., "stack the blue block") and low-level geometric trajectories. Traditionally, TAMP outputs a full trajectory of joint angles. While effective in static laboratory settings, this approach often fails in dynamic real-world environments. If an object moves mid-motion or if sensor noise introduces inaccuracies, the pre-calculated joint trajectory becomes obsolete, necessitating a slow re-planning cycle that can take seconds or minutes.
This paper, presented at ICRA 2020, introduces a flexible framework that shifts from absolute joint-space planning to relative object-centric planning. By defining goals in Cartesian space relative to objects, robots can handle dynamic changes and perception noise without constant re-planning.
System Architecture
The proposed framework bridges the gap between "slow" high-level planning and "fast" low-level reactive control through a two-loop architecture.
- Planning Loop: A STRIPS-based symbolic planner generates an action skeleton, which a frame optimizer then refines into an optimal sequence of relative Cartesian frames.
- Execution Loop: A path planner generates trajectories based on these optimal frames, which are then tracked by a reactive controller (Operational Space Control) running at 1000Hz. This high-frequency loop allows the robot to maintain relative poses even if objects are moving.
Object-Centric Formulation
The core innovation is the Cartesian Formulation of Logic Geometric Programming (LGP). Instead of defining a goal as a specific robot arm configuration, the planner defines it as a 6-DOF pose ($\xi$) of a "control frame" relative to a "target frame."
When a robot picks up an object, the kinematic tree of the world is updated: the object becomes a child of the gripper. If that object is then used as a tool (e.g., a hook to pull a box), the box's pose is calculated relative to the tool. This hierarchical modeling ensures that the "instruction" (e.g., "stay 5cm above the box") remains valid regardless of the box's global position. This approach also decouples actions temporally; the "place" action is no longer mathematically tied to the specific joint configuration of the "pick" action, simplifying the optimization problem.
Experimental Evaluation
The system was evaluated on two challenging manipulation benchmarks:
- Tower of Hanoi: A logic puzzle requiring the stacking of three blocks. This tested the planner's ability to handle long-term symbolic constraints and stacking stability.
- Workspace Reach: A tool-use scenario where the robot must use a hook to pull a distant box into its workspace before placing it on a shelf.
The framework solved the Tower of Hanoi (14 actions) in approximately 4 seconds and the Workspace Reach task in 3.7 seconds. Real-world experiments on a 7-DOF Franka Panda robot demonstrated robustness to manual object displacement and noisy perception.
Discussion and Future Work
While the object-centric approach offers significant advantages in dynamic environments, some challenges remain. The current high-level plan focuses on object frames and does not explicitly guarantee collision avoidance for the robot's entire body, relying instead on low-level potential fields. Additionally, the geometric constraints for actions like "push" or "pick" are currently manually defined. Future research could explore learning these constraints from demonstration and implementing closed-loop symbolic planning to handle the complete removal of objects from a scene.
Summary updated