Demonstration-guided Optimal Control
for Long-term Non-prehensile Planar Manipulation
Teng Xue, Hakan Girgin, Teguh Santoso Lembono, and Sylvain Calinon
Idiap Research Institute, Martigny, Switzerland
Abstract:
Long-term non-prehensile planar manipulation is a challenging task for robot planning and feedback control. It is characterized by underactuation, hybrid control, and contact uncertainty. One main difficulty is to determine both the continuous and discrete contact configurations, e.g., contact points and modes, which requires joint logical and geometrical reasoning. To tackle this issue, we propose a demonstration-guided hierarchical optimization framework to achieve offline task and motion planning (TAMP). Our work extends the formulation of the dynamics model of the pusher-slider system to include separation mode with face switching mechanism, and solves a warm-started TAMP problem by exploiting human demonstrations. We show that our approach can cope well with the local minima problems currently present in the state-of-the-art solvers and determine a valid solution to the task. We validate our results in simulation and demonstrate its applicability on a pusher-slider system with a real Franka Emika robot in the presence of external disturbances.
Demonstration Collection:
We used Matlab to build an interface to collect human demonstrations for the planar pushing task. Three representative demonstrations were performed, corresponding to different face-switching times (Ns=0, 1, 2).
The human expert uses the mouse to actuate the robot end-effector (red circle) to push the block, under the dynamic constraints. The end-effector velocity and acceleration are computed based on the mouse position and current end-effector position. The strategy (where and when) to switch the contact face is implicitly represented by the demonstrated velocity/acceleration as well.
Note: Gray, Green and blue colors represent the contact mode (sticking, sliding up, and sliding down, respectively). The magenta arrow represents the pusher velocity scale and direction. The cyan lines are the motion cones of each time step.
Dem 1: [15cm, -10cm, -90°]
Dem 2: [0, -20cm, 90°]
Dem 3: [15cm, -15cm, 90°]
Experiments:
Line Tracking
Face Switching
(Ns: number of face switches)