Constant-Time Motion Planning with Manipulation Behaviors

TL;DR

This paper introduces B-CTMP, a novel constant-time motion planning algorithm that integrates manipulation behaviors with collision-free planning, enabling fast, reliable, and verifiable robotic manipulation in semi-structured environments.

Contribution

B-CTMP extends existing constant-time motion planning to include manipulation behaviors, providing a unified, fast, and complete planning framework with guarantees.

Findings

B-CTMP achieves millisecond query times for manipulation tasks.

The algorithm guarantees success over a set of states in simulation and real robot tests.

It unifies collision avoidance and manipulation within a single framework.

Abstract

Recent progress in contact-rich robotic manipulation has been striking, yet most deployed systems remain confined to simple, scripted routines. One of the key barriers is the lack of motion planning algorithms that can provide verifiable guarantees for safety, efficiency and reliability. To address this, a family of algorithms called Constant-Time Motion Planning (CTMP) was introduced, which leverages a preprocessing phase to enable collision-free motion queries in a fixed, user-specified time budget (e.g., 10 milliseconds). However, existing CTMP methods do not explicitly incorporate the manipulation behaviors essential for object handling. To bridge this gap, we introduce the \textit{Behavioral Constant-Time Motion Planner} (B-CTMP), an algorithm that extends CTMP to solve a broad class of two-step manipulation tasks: (1) a collision-free motion to a behavior initiation state, followed by (2) execution of a manipulation behavior (such as grasping or insertion) to reach the goal. By precomputing compact data structures, B-CTMP guarantees constant-time query in mere milliseconds while ensuring completeness and successful task execution over a specified set of states. We evaluate B-CTMP on two canonical manipulation tasks, shelf picking and plug insertion, in simulation and on a real robot. Our results show that B-CTMP unifies collision-free planning and object manipulation within a single constant-time framework, providing provable guarantees of speed and success for manipulation in semi-structured environments.