ReVAMP: Vector-Accelerated Motion Planning for Kinematically-Constrained Systems via Reparameterization
Authors: Shrutheesh R. Iyer, Thomas Cohn, Zachary Kingston
Organizations: Department of Computer Science, Purdue University · Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology
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.
Figures & tables
Figure 1 : A 23-DoF bimanual mobile manipulator performing a whole-body pick-and-place task with motion plans generated by our motion planner.
Figure 2 : Vectorized Edge-validation in ReVAMP. Given ps and ptarget in parameterized space, they are interpolated, that are then validated in parallel. There are two stages here (a) Early-collision checking pre-filter by eliminating edges where the interpolated P-space points are in collision with the environment. (b) If EEFs are collision free, then IK is resolved in parallel and a full C-space collision check is performed. n=4 (unit of parallelization) here for illustrative purposes
Algorithm 1 ParameterizedExtend
Figure 4
Figure 4 : Maze solver results: (a) planning times to solve the maze (b) Task-space and configuration space distance
Figure 5 : Real-world maze-setup, with obstacles that block the maze and the C -space.
Metric
McVAMP
ReVAMP
Success (%)
91.89
95.65
Planned z-error (mm)
1.020 (1.227 ± 0.988)
0.000 (0.000 ± 0.000)
Executed z-error (mm)
1.075 (1.307 ± 0.966)
0.191 (0.221 ± 0.168)
Plan. time (ms)
146.19 (500.90 ± 666.05)
11.95 (37.72 ± 67.07)
Iterations
10292 (30745 ± 39038)
4570 (11930 ± 17295)
Table II : Real-Maze results. Values are median (mean ± std).
Figure 6 : Bimanual transport results: (a) planning times (b) Task-space and configuration space distance
Method
Iterations
Planning (ms)
Shortcut (ms)
Total (ms)
Config dist. (rad)
EEF dist.
McVAMP
310 (437 ± 358)
2.80 (3.56 ± 2.68)
0.02 (0.07 ± 0.14)
2.89 (3.63 ± 2.73)
14.12 (14.03 ± 4.67)
6.17 (6.22 ± 2.45)
LeaderFollower
1618 (2249 ± 2057)
0.36 (0.45 ± 0.35)
0.08 (0.09 ± 0.04)
0.44 (0.53 ± 0.36)
7.06 (7.30 ± 1.37)
2.63 (2.77 ± 0.71)
DualFollower
58 (75 ± 68)
0.11 (0.15 ± 0.13)
0.02 (0.02 ± 0.02)
0.13 (0.17 ± 0.14)
5.54 (5.68 ± 1.50)
1.14 (1.17 ± 0.32)
Table III : Median (mean ± std) planning cost and shortcut path length per method.
Metric
IFT (baseline)
Mod. IFT + McVAMP
Mod. IFT + ReVAMP
Pipeline Result
Success Rate
98% (39/40)
95% (38/40)
100% (40/40)
Time to Plan Median (s)
50.2
36.7
21.2
Time to Plan Mean (s)
70.2
41.8
25.0
Time to Plan Max (s)
218.2
84.6
75.5
Path Length (rad)
17.25
16.83
17.95
Table IV : RB-Y1 box pickup planning results: the original IFT pipeline [ 6 ] as a baseline, versus our modified pipeline instantiated with McVAMP and ReVAMP as the constrained-planning backend.
Parasol Lab, School of Computing and Data Science, University of Illinois at Urbana Champaign, Champaign, IL, 61820 USA · Department of Computer Science at Instituto Tecnológico Autónomo de México (ITAM), Mexico City, México