Multi-query Robotic Manipulator Task Sequencing with Gromov-Hausdorff Approximations

Robotic manipulator applications often require efficient online motion planning. When completing multiple tasks, sequence order and choice of goal configuration can have a drastic impact on planning performance. This is well known as the robot task sequencing problem (RTSP). Existing general purpose RTSP algorithms are susceptible to producing poor quality solutions or fail entirely when available computation time is restricted. We propose a new multi-query task sequencing method designed to operate in semi-structured environments with a combination of static and non-static obstacles. Our method intentionally trades off workspace generality for planning efficiency. Given a user-defined task space with static obstacles, we compute a subspace decomposition. The key idea is to establish approximate isometries known as $\epsilon$-Gromov-Hausdorff approximations that identify points that are close to one another in both task and configuration space. Importantly, we prove bounded suboptimality guarantees on the lengths of trajectories within these subspaces. These bounding relations further imply that trajectories within the same subspace can be smoothly concatenated which we show is useful for determining efficient task sequences. We evaluate our method with several kinematic configurations in a complex simulated environment, achieving up to 3x faster motion planning and 5x lower maximum trajectory jerk compared to baselines.