Vectorizing Projection in Manifold-Constrained Motion Planning for Real-Time Whole-Body Control

Shrutheesh R Iyer; I-Chia Chang; Andrew Z. Liu; Yan Gu; Zachary Kingston

cs.ROApr 14, 2026

Vectorizing Projection in Manifold-Constrained Motion Planning for Real-Time Whole-Body Control

Authors: Shrutheesh R Iyer, I-Chia Chang, Andrew Z. Liu, Yan Gu, Zachary Kingston

Organizations: Department of Computer Science, Purdue University · School of Mechanical Engineering, Purdue University

Abstract

Many robot planning tasks require satisfaction of one or more constraints throughout the entire trajectory. For geometric constraints, manifold-constrained motion planning algorithms are capable of planning collision-free path between start and goal configurations on the constraint submanifolds specified by task. Current state-of-the-art methods can take tens of seconds to solve these tasks for complex systems such as humanoid robots, making real-world use impractical, especially in dynamic settings. Inspired by recent advances in hardware accelerated motion planning, we present a CPU SIMD-accelerated manifold-constrained motion planner that revisits projection-based constraint satisfaction through the lens of parallelization. By transforming relevant components into parallelizable structures, we use SIMD parallelism to plan constraint satisfying solutions. Our approach achieves up to 100-1000x speed-ups over the state-of-the-art, making real-time constrained motion planning feasible for the first time. We demonstrate our planner on a real humanoid robot and show real-time whole-body quasi-static plan generation. Our work is available at https://commalab.org/papers/mcvamp/.

Figures & tables

Figure 1: Real-time quasi-static whole-body planning with our vectorized planning approach on the Digit humanoid. In this task, the robot must transport a box between the three levels of the shelf, which requires whole body coordination, obstacle avoidance, and constraint satisfaction. Our planner finds feasible motion from start to goal in this scenario in less than 12 milliseconds on average, practical for real-time planning.

Panel of Figure 2 — Algorithm 1 Vectorized Manifold-Constrained RRT-Connect

Panel of Figure 3 — Algorithm 2 ParallelConstrainedExtend

Figure 4: Example of a line problem generated with start goal pairs (spheres) and the solution. Random cuboid obstacles are generated around the robot which have to be avoided.

Panel of Figure 6 — Figure 6: Solving a Maze. Here the tip of the marker is constrained to the floor of the maze. Consequently, the robot has to solve the maze to find a plan from start to goal. Planning is done at 20Hz. (b) when a dynamic obstacle blocks the path (red sphere here), the planner can compute a new path due to the high motion validation and planning throughput

Method	Mean	Q1	Median	Q3	95%	Succ.
OMPL	25.10	7.33	17.323	29.20	85.82	72%
OMPL (comp.)	13.25	1.00	3.29	16.80	52.07	76%
McVAMP	0.032	0.002	0.006	0.014	0.058	82%

Table I : Results for maze Solving. All times are reported in seconds . OMPL (comp.) refers to the OMPL planner with our compiled projection.

Method	T $\to$ M	M $\to$ B	B $\to$ T
IK-BiRRT	96.74 $\pm$ 48.48	90.11 $\pm$ 21.70	121.90 $\pm$ 53.61
	(3.69 $\pm$ 0.63)	(5.64 $\pm$ 0.96)	(6.26 $\pm$ 1.09)
IK-GCS	63.47 $\pm$ 1.95	44.18 $\pm$ 2.74	77.87 $\pm$ 1.19
	( 2.52 $\pm$ 0.0 )	( 4.73 $\pm$ 0.0 )	( 5.82 $\pm$ 0.0 )
McVAMP	2.11 $\pm$ 1.16	3.53 $\pm$ 2.44	2.36 $\pm$ 1.63
(1-step)	(7.72 $\pm$ 1.50)	(6.08 $\pm$ 1.51)	(8.93 $\pm$ 1.33)

Table II: KUKA IIWA Bimanual Planning Results: Time in milliseconds (top) and Path Cost (bottom in parenthesis).

Figure 7: The different constraints of the Digit robot. (i) $\mathcal{C}_{B}$ , the bimanual fixed pose constraint, (ii) $\mathcal{C}_{CL}$ , the closed linkage constraint, (iii) $\mathcal{C}_{COM}$ , the center of mass constraint, and (iv) $\mathcal{C}_{F}$ , the fixed feet placement constraint.

Motion phases	S $\to$ T	T $\to$ B	B $\to$ M	M $\to$ T
Tracking error (deg)
Left Knee Joint	1.70 $\pm$ 1.57	0.64 $\pm$ 0.38	1.28 $\pm$ 0.34	1.00 $\pm$ 0.50
Left Tarsus Joint	4.37 $\pm$ 2.26	4.80 $\pm$ 1.82	8.63 $\pm$ 1.73	6.38 $\pm$ 1.31
Right Knee Joint	3.81 $\pm$ 1.78	2.57 $\pm$ 1.05	5.61 $\pm$ 0.71	4.51 $\pm$ 0.63
Right Tarsus Joint	3.51 $\pm$ 1.73	5.15 $\pm$ 1.93	7.74 $\pm$ 1.93	5.85 $\pm$ 1.89
Planning Time (ms)	4.93 $\pm$ 0.12	10.11 $\pm$ 1.65	13.42 $\pm$ 3.68	16.28 $\pm$ 2.11

Table III : Planning to transport a box. The robot picks up the box from the top shelf, transports it to the bottom shelf, then the middle and finally the top one. We show the mean tracking error in the legs between the controller and the planner, and planning time in milliseconds . All numbers are reported as mean

\pm

1 standard deviation.

Panel of Figure 8 — Figure 8: Experimental results for dynamic environment obstacle avoidance.

Motion Planning

Motion planning finds safe, feasible paths and trajectories for robots and vehicles amid geometric, dynamic, and task constraints.

See all 392 papers

PATACON: GPU-Parallel Tangent-Bundle RRT-Connect for Constrained Motion Planning
Oct 9, 2026Gahyun Oh, Junho An, Suhyun Jeon +1Constrained Motion Planning Motion Planning
Department of Robotics, Kwangwoon University, Seoul, Republic of Korea. · Department of Intelligence and Information, Graduate School of Convergence Science and Technology, Seoul National University, Republic of Korea.
ReVAMP: Vector-Accelerated Motion Planning for Kinematically-Constrained Systems via Reparameterization
Sep 24, 2026Shrutheesh R. Iyer, Thomas Cohn, Zachary KingstonConstrained Motion Planning Robotic Manipulation
Department of Computer Science, Purdue University · Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology
VAMP-MR: Vector-Accelerated Motion Planning and Execution for Multi-Robot-Arms
Jul 15, 2026Philip Huang, Chenrui Gao, Jiaoyang LiMulti-Robot Motion Planning Robotic Manipulation
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA · University of Michigan, Ann Arbor

Oct 9, 2026J/K move · Enter open · S save

PATACON: GPU-Parallel Tangent-Bundle RRT-Connect for Constrained Motion Planning

Gahyun Oh, Junho An, Suhyun Jeon, Suhan Park

Department of Robotics, Kwangwoon University, Seoul, Republic of Korea. · Department of Intelligence and Information, Graduate School of Convergence Science and Technology, Seoul National University, Republic of Korea.

Constrained Motion Planning Motion Planning Robot Motion Planning Sampling-Based Motion Planning

Open paper AskPDF arXiv

Fast whole-body motion planning enables continuous replanning for humanoid robots in dynamic environments while satisfying collision avoidance and kinematic constraints. However, constrained motion planning incurs substantial computational cost due to repeated constraint projection in addition to collision checking and nearest-neighbor search. Recently, GPU-based massively parallel planners have been proposed to reduce computational cost. Nevertheless, these planners still steer in the ambient configuration space, so projecting candidates that deviate from the manifold remains a significant source of overhead, particularly in highly constrained, high-dimensional problems. In this work, we introduce PATACON, a GPU-parallel bidirectional planner that explores tangent space to exploit the local manifold geometry induced by known constraint functions. Experiments on dual-arm closed-chain and humanoid whole-body constrained planning tasks show that PATACON outperforms the prior work as configuration-space dimension and constraint codimension grow, achieving up to an 11.25 times speedup. These results suggest that GPU-parallel tangent-space exploration brings constrained whole-body motion planning closer to real-time performance.

Abstract

Motion Planning

Explore similar work

PATACON: GPU-Parallel Tangent-Bundle RRT-Connect for Constrained Motion Planning

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

VAMP-MR: Vector-Accelerated Motion Planning and Execution for Multi-Robot-Arms