Fast Task and Motion Planning for Multiple Manipulators

Abstract

Industrial manipulators are increasingly used in manufacturing and packaging systems where the exact tasks are not known in advance. In such settings, task and motion planning (TAMP) must be performed online, while the resulting plans should maximize throughput and satisfy velocity, acceleration, and higher-order derivative constraints. This is especially challenging when multiple fast manipulators operate close together in a shared workspace, resulting in additional constraints on trajectories, task allocations, and orderings to avoid collisions.
Existing multi-agent TAMP methods often rely on discretized space and time, resulting in trajectories that cannot be executed directly by manipulators. On the other hand, continuous trajectory optimization methods can generate smooth trajectories, but they do not address task allocations and orderings, and either consider a single manipulator and/or are too slow for online planning.
We present a soft real-time TAMP framework for multiple fast manipulators. Tasks are generated from product and place positions, assigned to robots, and converted into dynamically feasible trajectories. Experiments compare polygonal bang-bang, polygonal smoothstep, and smooth optimized Bézier trajectories with continuity up to acceleration and jerk. The results show that the polygonal trajectories are the fastest to compute, while Bézier optimization reduces the makespan at the cost of increased planning time. Additionally, bang-bang initialization gives faster convergence than smoothstep and is thus a good starting point for fast Bézier optimization. The makespan improvement reduces per iteration, allowing for a relatively large improvement in minimal time. The proposed assignment methods are shown to be feasible for online planning resulting in few potential collisions, and a custom convex-hull-based collision checker is compared to a sample-based collision checker on the tradeoff between conservativeness and runtime.