ReVAMP: Vector-Accelerated Motion Planning for Kinematically-Constrained Systems via Reparameterization

Summary

Abstract Summary

Robots often must satisfy one or more constraints during motion planning for real-world tasks. When such constraints reduce the valid configuration space to a measure-zero subset, sampling based planning algorithms require modifications to draw feasible samples. For many common end-effector constraints, parameterizations built on inverse kinematics (IK) provide an alternate formulation where the constraints are satisfied by construction, allowing directly sampling the feasible set. Despite their elegant approach, parameterized planners have remained slower than vector-accelerated implementations of projection-based approaches, leaving their performance ceiling an open question. We explore a new axis of vectorization built upon reparameterizing the planning space through analytic IK. This approach addresses existing inefficiencies in vectorized projection-based planners and exposes new opportunities for parallelism within the planner. We show that the planner can synthesize plans in microseconds to milliseconds for high dimensional systems (up to 20 dimensions), with complex constraints, up to 10x faster than the current state-of-the-art. Furthermore, we demonstrate how such planning speeds open up avenues for restructuring sequential manipulation pipelines.

Key Points

Robots often must satisfy one or more constraints during motion planning for real-world tasks
When such constraints reduce the valid configuration space to a measure-zero subset, sampling based planning algorithms require modifications to draw feasible samples.
For many common end-effector constraints, parameterizations built on inverse kinematics (IK) provide an alternate formulation where the constraints are satisfied by construction, allowing directly sampling the feasible set.
Despite their elegant approach, parameterized planners have remained slower than vector-accelerated implementations of projection-based approaches, leaving their performance ceiling an open question.
We explore a new axis of vectorization built upon reparameterizing the planning space through analytic IK.

Preliminaries

Let $C\subset\mathbb{R}^{d}$ be the configuration space of a robot with $d$ degrees of freedom, where a configuration $q\in C$ is typically the vector of actuated joint angles. In the presence of environment obstacles, $C_{\text{free}}\subset C$ denotes the subset of configurations that are not in collision. A robot typically interacts with the environment through its end-effector(s), EE(s).

Methodology

In this work, we propose a new compilation axis for a robot: the analytic inverse kinematics routine, and the subsequent reparameterization that is constructed from it. Starting from the analytic solver, we transform the generated routines into branchless and vectorizable units that map parameterized configurations into joint configurations in parallel.

This allows multiple configurations in the reparameterized space to be resolved simultaneously using SIMD parallelism.

Methodology — ReVAMP: Vector-Accelerated Motion Planning for Kinematically-Constrained Systems via Reparameterization — Figure 5 : Real-world maze-setup, with obstacles that block the maze and the $C$-space.

Experimental Evaluation

We evaluate our planner across different robots, with increasing DoF, and constraint complexity. All methods were evaluated on the same set of problems on a 5.4GHz Intel i7-13700K CPU with 64GB of RAM, unless specified otherwise. Each experiment highlights a distinct benefit of the planner and its potential implications.

For the FR3 with a plane constraint, we show that planning on $SE(2)$ provides better scaling and strict constraint satisfaction.

Sources and Demonstrations

Figures are reproduced from Iyer et al. See the full paper for experimental details and the project page, when the paper links one, for demonstrations and videos.