Constraint Satisfaction for Motion Feasibility Checking

Task and motion planning (TAMP) is a key research field for robotic manipulation tasks. The goal of TAMP is to generate motion-feasible task plan automatically. Existing methods for checking motion feasibility of task plan skeletons have some limitations of semantic-free pose candidate sampling, weak search heuristics, and early value commitment. In order to overcome these limitations, we propose a novel constraint satisfaction framework for checking motion feasibility of task plan skeletons. Our framework provides (1) a semantic pose candidate sampling method, (2) novel variable and constraint ordering heuristics based on intra- and inter-action dependencies in a task plan skeleton, and (3) an efficient search strategy using constraint propagation. Based upon these techniques, our framework can improve the efficiency of motion feasibility checking for TAMP. From experiments using the humanoid robot PR2, we show that the motion feasibility checking in our framework is 1.4x to 6.0x faster than previous ones.