Multi-step manipulation task and motion planning guided by video demonstration

TL;DR

This work tackles multi-step task and motion planning for manipulation by leveraging instructional video to guide planning. It extracts contact state changes and 6D object poses from video using a hand contact recognizer and CosyPose, forming an admissible configuration space of placement and grasp states that bias the search. A multi-tree RRT explores transitions between states, followed by trajectory refinement through an optimal-control-based solver to produce smooth, executable motions. The authors introduce a benchmark with shelf, tunnel, and waiter tasks, and show that video guidance improves success rates and enables solving challenging scenarios, with demonstrated generalization to new poses, objects, and environments and real-robot execution aided by trajectory refinement.

Abstract

This work aims to leverage instructional video to solve complex multi-step task-and-motion planning tasks in robotics. Towards this goal, we propose an extension of the well-established Rapidly-Exploring Random Tree (RRT) planner, which simultaneously grows multiple trees around grasp and release states extracted from the guiding video. Our key novelty lies in combining contact states and 3D object poses extracted from the guiding video with a traditional planning algorithm that allows us to solve tasks with sequential dependencies, for example, if an object needs to be placed at a specific location to be grasped later. We also investigate the generalization capabilities of our approach to go beyond the scene depicted in the instructional video. To demonstrate the benefits of the proposed video-guided planning approach, we design a new benchmark with three challenging tasks: (I) 3D re-arrangement of multiple objects between a table and a shelf, (ii) multi-step transfer of an object through a tunnel, and (iii) transferring objects using a tray similar to a waiter transfers dishes. We demonstrate the effectiveness of our planning algorithm on several robots, including the Franka Emika Panda and the KUKA KMR iiwa. For a seamless transfer of the obtained plans to the real robot, we develop a trajectory refinement approach formulated as an optimal control problem (OCP).

Figures (9)

• Figure 1: The proposed planning approach is guided by the demonstration video (A). The video depicts a person manipulating a known object; the cheez-it box in this particular example. The video can contain several pick-and-place actions with multiple objects. Here, only a short clip with only one object and one action is shown. From the video, we recognize (i) the contact states between the human hand and the object, marked by red bounding boxes in (B); and (ii) the object 6D pose (3D translation and 3D rotation w.r.t camera) at the grasp and release contact states, marked in yellow in (B). The robot trajectory planned by the proposed approach is shown in (C). The start and goal object poses in (C) are shown in magenta and green, respectively.
• Figure 2: Approach overview. (i) First, we extract contact states and 6D object poses from the input instructional video, as described in Sec. \ref{['sec:extracting_contacts_poses']}. (ii) Next, we grow multiple trees in the admissible configuration space until we find a path between the start and goal configurations. More details on the state space are in Sec. \ref{['sec:state_space']}, and more details on planning the path in Sec. \ref{['subsec:plan_between_states']}. (iii) This path is then further refined by an optimization module and (iv) executed either in simulation (iv-a) or on a real-world robot (iv-b).
• Figure 3: The admissible configuration space is a set of (i) placement states$\mathcal{P}_i$ where objects rest on the contact surfaces at specific poses and (ii) grasp states$\mathcal{G}_j$ where one of the objects is grasped by the robot gripper. To transit to another placement state, i.e. to change the pose of one object, the robot needs to grasp the object and release it at the new location. Since we define several ways to grasp each object, we can transition through multiple grasp states to build a path between configurations given by two consecutive placement states. Start configuration (A) lies in the first placement state $\mathcal{P}_1$. To achieve the goal, the robot needs to move the brown object first, i.e. to reach state $\mathcal{P}_2$ (e.g. configuration (B)). Finally, the robot moves the second object (reach $\mathcal{P}_3$) and moves robot configuration to the given goal (C). The configuration at the transition between the placement state and grasp state is not unique as there are various robot joint values resulting in the same 'pregrasp' poses as shown in (D). Multiple ways of grasping the same object are represented by different grasp states as shown in (E). The video demonstration provides object poses (see Sec. \ref{['sec:extracting_contacts_poses']}) to construct the placement states $\mathcal{P}_1$, $\mathcal{P}_2$, $\mathcal{P}_3$ (see Sec. \ref{['sec:state_space']}). Finally, the planner spawns and expands multiple trees (see Sec. \ref{['subsec:plan_between_states']}) until it finds the path between start and goal configurations.
• Figure 4: Generalization scenarios include variation of: start object poses (\ref{['subfig:generalization_object_start']}), goal object poses (\ref{['subfig:generalization_object_goal']}), object type (\ref{['subfig:all_possible_objects']}) and environment (\ref{['subfig:generalization_env']}). \ref{['subfig:generalization_object_start']}, \ref{['subfig:generalization_object_goal']}: start and goal object pose variation. We sample randomly $x$ and $y$ displacements for object start (goal) poses and a rotation around $z$ axis. \ref{['subfig:all_possible_objects']}: Object variation. We illustrate the variability of objects that our method supports. \ref{['subfig:generalization_env']}: Environment variation. Furniture pose variability for shelf, tunnel, and waiter tasks. We sample randomly $x$ and $y$ displacements for the tunnel in the tunnel task; $x$, $y$, and $z$ axis displacements for the shelf in the shelf task and the table in the waiter task.
• Figure 5: The proposed benchmark includes three tasks: the shelf task (a) with the goal of moving several objects to a predefined pose on the table or on the shelf; the tunnel task (b) with the goal of transferring an object through the tunnel; the waiter task (c) with the goal of moving several objects to a distant location while using the tray to minimize the traveled distance.