Abstract
This paper introduces a novel approach to Task and Motion Planning (TAMP) that utilizes human instructional videos to guide robots through complex, multi-step manipulation tasks. By extracting contact states and 3D object poses from video, the researchers seed a multi-tree RRT planner that can solve challenging problems—such as navigating narrow passages or managing sequential dependencies—far more efficiently than traditional "blind" solvers.
Introduction
Robots frequently struggle with Task and Motion Planning (TAMP). While motion planning (finding a path from A to B) is relatively straightforward, task planning involves determining a sequence of discrete actions, such as picking up a box and placing it on a shelf to adjust grip before moving it through a narrow tunnel. The search space for these problems is massive, combining continuous joint movements with discrete grasping actions.
Standard algorithms often fail in "narrow passages"—specific, constrained configurations that are difficult to find via random sampling. Humans perform these tasks intuitively; this research aims to bridge the gap by using human demonstrations to identify critical transition points (grasps and releases) for the robot, providing a "skeleton" for the plan while letting the robot calculate its own joint configurations.
Methodology
The proposed pipeline moves from raw video to a finalized robot trajectory through four key stages:
- Video Preprocessing: A hand contact recognizer and a 6D pose estimator (CosyPose) identify when a human touches an object and its 3D coordinates at that moment.
- Multi-Contact RRT: This core algorithmic contribution extends the Rapidly-Exploring Random Tree (RRT) algorithm to grow multiple trees simultaneously. These trees are rooted at the "neighboring states" identified in the video—the specific points where an object transitions between surfaces and hands.
- Admissible Configuration Space: The planner restricts its search to manifolds where physical constraints are met (e.g., stable resting or firm gripping).
- Path Optimization: A post-processing module shortens the path to remove redundant or jerky motions typical of sampling-based planners.
Experimental Setup
The method was evaluated using objects from the standard YCBV dataset and a new benchmark featuring three high-difficulty tasks:
- The Shelf Task: Rearranging objects between a table and a shelf (testing 3D spatial reasoning).
- The Tunnel Task: Moving an object through a narrow opening requiring a specific orientation (testing narrow passage navigation).
- The Waiter Task: Using a mobile robot (KUKA KMR iiwa) to move objects on a tray (testing mobile base and arm coordination).
Key Results
The video-guided planner was tested against state-of-the-art baselines like HPP (Humanoid Path Planner) and PDDLStreams.
- Success Rate and Efficiency: In the "Shelf" task with three objects, the proposed method maintained a high success rate while standard HPP failed completely within the 60-second limit.
- Narrow Passage Navigation: The planner successfully solved the "Tunnel" task by leveraging the human's demonstration of the correct object orientation, a classic failure point for sampling planners.
- Cross-Platform Robustness: The system proved effective across different platforms, including the 7-DoF Franka Emika Panda and the mobile KUKA KMR iiwa.
Conclusion and Future Work
While powerful, the system requires known 3D models of objects and environments for pose estimation and collision checking. It is currently limited to pick-and-place actions and is sensitive to occlusions where the human hand hides the object during critical transitions. Future research will focus on extending the framework to non-prehensile manipulations like pushing or sliding and improving the vision pipeline to handle significant occlusions.
Summary updated