Constant-Time Motion Planning with Manipulation Behaviors

TL;DR

This work addresses real-time, verifiable planning for tasks that combine collision-free motion with manipulation behaviors in semi-structured environments. It extends Constant-Time Motion Planning (CTMP) by embedding manipulation behaviors into offline preprocessing, yielding Behavioral CTMP (B-CTMP) with constant-time online queries and formal completeness guarantees within a region of interest. The method uses a compact library of attractor tuples and neighborhoods to cover the object pose space, and validates behavior rollouts offline to ensure executable plans online. Experiments on shelf grasping and charger insertion show robust performance, fast decision times, and reduced memory usage compared with baseline approaches. The work introduces PR-completeness as a framework for reasoning about guarantees in behavior-based manipulation within fixed RoIs.

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 in simulation, shelf picking and plug insertion,and demonstrate its effectiveness 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.

Figures (3)

• Figure 1: Shelf-picking task commonly encountered in industrial warehouse automation. B-CTMP performs a preprocessing phase to compute compact data structures that enable finding a two-phase plan in constant time during online planning. In preprocessing, it caches a set of representative tuples, each containing a path from the robot’s home state to an attractor initiation state (red curve), from which a behavior policy can be executed to reach the target (green curves).
• Figure 2: Overview of the preprocessing structure. The planner computes a reduced set of feasible initiation states $\widetilde{\mathcal{S}}$, each connected to the robot’s home state $s_{home}$ by a precomputed path. Each $s_i \in \widetilde{\mathcal{S}}$ defines a neighborhood $n_i(s_i)$ within the region of interest $\mathcal{G}$ of object states $w$ that can be reached through behavior execution. This compression is captured by attractor tuples, each representing an initiation state, its reachable neighborhood, and the stored path that enables efficient plan retrieval during online execution.
• Figure 3: Experimental comparison of B-CTMP against baseline methods on the two tasks. (\ref{['fig:subfig1']}) The shelf grasping environment, where a fixed-base robot must grasp a known target object at any location on the shelf. (\ref{['fig:subfig4']}) The charger insertion environment, where the robot must precisely insert a charger into a port located anywhere within the confined box, with pose uncertainty due to perception noise. (\ref{['fig:subfig5']}) Success rates for collision-free motion planning and subsequent behavior execution show that baseline methods exhibit 10-30% failure rates due to unsuccessful behavior rollouts, while B-CTMP maintains consistently high performance. (\ref{['fig:subfig6']}) Planning times demonstrate B-CTMP's sub-millisecond online query performance through fast lookup operations, compared to expensive online computation required by baseline methods.

Theorems & Definitions (6)

• Definition 1: Behavior Feasibility
• Definition 2: Constant-Time Feasibility
• Definition 3: Initiation state Coverage
• Definition 4: Neighborhoods
• Definition 5: PR-Completeness
• Theorem 1: B-CTMP PR-Completeness