This paper, presented at ICRA 2020, introduces a more flexible way for robots to plan complex tasks in worlds that don't stand still. By shifting from absolute "joint-space" planning to relative "object-centric" planning, the authors enable robots to handle moving objects and sensor noise without getting stuck in a loop of constant re-planning.
TL;DR
The authors propose a Task and Motion Planning (TAMP) framework that optimizes over relative Cartesian frames rather than absolute joint trajectories. This allows a robot to generate high-level plans that remain valid even if objects move, enabling seamless integration with reactive, real-time controllers.
What Problem Does This Paper Address?
Task and Motion Planning (TAMP) is the "brain" of a robot, combining symbolic logic (e.g., "stack the blue block on the red one") with geometric trajectories (e.g., "move the gripper to coordinates X, Y, Z").
Traditionally, TAMP algorithms produce a full trajectory of joint angles. This works in a static lab, but fails in the real world for two reasons:
- Dynamic Environments: If a human moves a block while the robot is mid-motion, the pre-calculated joint trajectory becomes useless. Re-planning typically takes seconds or minutes—far too slow for a robot to "react."
- Perception Uncertainty: If a camera's estimate of an object's position is off by a few centimeters, an open-loop joint trajectory will miss the grasp entirely.
The authors aim to bridge the gap between high-level "slow" planning and low-level "fast" reactive control.
Methods
The core innovation is a Cartesian Formulation of Logic Geometric Programming (LGP).
- Relative Frames: Instead of defining a goal as a specific arm configuration, the planner defines the goal as a 6-DOF pose ($\xi$) of a "control frame" (like the gripper) relative to a "target frame" (like a block).
- Kinematic Tree Optimization: The robot's world is modeled as a tree of frames. When a robot picks up a hook, the hook becomes a child of the gripper. If the robot then uses that hook to pull a box, the box's pose is calculated relative to the hook.
- Mathematical Derivation: The authors derive the $SE(3)$ Jacobians necessary to optimize these relative frames using gradient-based solvers (like IPOPT). This allows the optimizer to find the "best" way to grasp or place an object by minimizing a cost function (like total distance traveled).
- Reactive Execution: Because the plan is defined relatively (e.g., "stay 5cm above the box center"), the robot can use Operational Space Control (OSC). This low-level controller runs at 1000Hz, constantly adjusting the motors to maintain that relative relationship even if the box is sliding across the table.
Datasets
As is common in robotics research, the paper does not use a static dataset but evaluates the system on two benchmark manipulation problems:
- Tower of Hanoi: A logic puzzle requiring the robot to stack three blocks in a specific order. This tests the planner's ability to handle long-term symbolic constraints and stacking stability.
- Workspace Reach: A "tool use" scenario where a box is placed outside the robot's reach. The robot must plan to pick up a hook, use it to push the box into its workspace, and then place the box on a shelf.
Key Insights and Ideas
- Invariance to Motion: By planning in relative Cartesian space, the high-level plan becomes invariant to the object's global position. The "instruction" doesn't change just because the object moved.
- Temporal Decoupling: In joint-space planning, the "place" action is mathematically coupled to the "pick" action (how you hold it determines where your elbow is when you drop it). In this object-centric view, these constraints are decoupled, simplifying the optimization problem and making it more robust.
- The "Aha" Moment: The realization that TAMP doesn't need to output a trajectory; it only needs to output the optimal relative poses at key "switch" points. The low-level controller can fill in the gaps reactively.
Key Results
- Efficiency: The framework solved the complex Tower of Hanoi (14 actions) in about 4 seconds and the Workspace Reach task in 3.7 seconds using the IPOPT solver.
- Real-World Robustness: The authors demonstrated the system on a 7-DOF Franka Panda robot. The robot successfully completed tasks even when objects were moved by hand during execution and when perception was intentionally noisy.
- Solver Performance: The study compared IPOPT and NLOPT, finding that IPOPT (an interior-point method) was significantly more reliable at finding feasible solutions for these highly non-linear geometric constraints.
Limitations and Future Work
- Collision Avoidance: Because the high-level plan focuses on object frames rather than the robot's "body," it doesn't explicitly guarantee that the robot's elbow won't hit an obstacle. The authors currently rely on low-level potential fields to avoid collisions, which might not work in very cluttered spaces.
- Joint Limits: Similarly, a Cartesian plan might ask for a pose that is technically reachable but forces the robot into a "singular" configuration or hits a joint limit.
- Manual Action Design: Currently, the geometric constraints for actions like "push" or "pick" must be manually defined by engineers. The authors suggest that future work could involve learning these constraints from demonstration.
- Closed-Loop Planning: While the execution is reactive, the symbolic plan is still generated once at the start. Ideally, the symbolic planner would also run in a loop to handle cases where an object is removed from the scene entirely.
Summary updated