Continuous Optimization-Based Task and Motion Planning with Signal Temporal Logic Specifications for Sequential Manipulation

We propose a new optimization-based task and motion planning (TAMP) with signal temporal logic (STL) specifications for robotic sequential manipulation such as pick-and-place tasks. Given a high-level task specification, the TAMP problem is to plan a trajectory that satisfies the specification. This is, however, a challenging problem due to the difficulty of combining continuous motion planning and discrete task specifications. The optimization-based TAMP with temporal logic specifications is a promising method, but existing works use mixed integer problems (MIP) and do not scale well. To address this issue, in our approach, a new hybrid system model without discrete variables is introduced and combined with smooth approximation methods for STL. This allows the TAMP to be formulated as a nonlinear programming problem whose computational cost is significantly less than that of MIP. Furthermore, it is also possible to deal with nonlinear dynamics and geometric constraints represented by nonlinear functions. The effectiveness of the proposed method is demonstrated with both numerical experiments and a real robot.