arxiv: 2604.13323 · v1 · submitted 2026-04-14 · 💻 cs.RO

Recognition: unknown

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

Authors on Pith no claims yet

Pith reviewed 2026-05-10 14:27 UTC · model grok-4.3

classification 💻 cs.RO

keywords manifold-constrained planningSIMD accelerationprojection methodswhole-body motion planninghumanoid robotsreal-time controlconstraint satisfaction

0 comments

The pith

Rewriting projections as parallel SIMD operations speeds up manifold-constrained robot planning by 100-1000x.

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The paper establishes that manifold-constrained motion planning, which requires keeping entire trajectories on specific geometric surfaces, can be made real-time by vectorizing the key projection steps that enforce those constraints. Instead of sequential calculations, the method uses CPU SIMD instructions to handle multiple projections simultaneously. This turns planning times from tens of seconds into milliseconds, which matters because it allows robots like humanoids to replan motions quickly in changing environments rather than relying on precomputed slow paths.

Core claim

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.

What carries the argument

The SIMD-parallelized projection operator that performs repeated projections onto constraint manifolds in a vectorized, simultaneous manner across data lanes to enforce geometric constraints during path search.

If this is right

Planning times for complex constrained tasks drop from tens of seconds to real-time rates.
Whole-body quasi-static plans for humanoid robots can be generated on the fly.
Dynamic environments become accessible because replanning no longer requires long waits.
Standard CPUs without specialized hardware can now handle these previously intractable problems.

Where Pith is reading between the lines

These are editorial extensions of the paper, not claims the author makes directly.

Similar vectorization could accelerate other robotics algorithms that rely on iterative projections or optimizations.
Testing on a wider range of robots and constraint types would show how broadly the speedup applies.
Combining this CPU method with occasional GPU offloading might handle even more demanding cases.

Load-bearing premise

That the SIMD restructuring of the projection steps does not change the numerical results or allow any constraint violations to slip through.

What would settle it

Running the vectorized planner on a standard benchmark task and observing either planning times no better than the original method or a generated path that violates one of the manifold constraints.

Figures

Figures reproduced from arXiv: 2604.13323 by Andrew Z. Liu, I-Chia Chang, Shrutheesh R Iyer, Yan Gu, Zachary Kingston.

**Figure 2.** Figure 2: Methodology. (a) RRT-Extend Step: A random configuration in ambient space is sampled. (b) [PITH_FULL_IMAGE:figures/full_fig_p004_2.png] view at source ↗

**Figure 3.** Figure 3: Cyclic Projection to intersection of manifolds. At each iteration, we [PITH_FULL_IMAGE:figures/full_fig_p005_3.png] view at source ↗

**Figure 5.** Figure 5: CDF plot benchmarking planner performance for the line and plane constrained problems, binned by number of environment obstacles. [PITH_FULL_IMAGE:figures/full_fig_p006_5.png] view at source ↗

**Figure 6.** Figure 6: Solving a Maze. Here the tip of the marker is constrained to the [PITH_FULL_IMAGE:figures/full_fig_p006_6.png] view at source ↗

**Figure 7.** Figure 7: The different constraints of the Digit robot. (i) [PITH_FULL_IMAGE:figures/full_fig_p007_7.png] view at source ↗

**Figure 8.** Figure 8: Experimental results for dynamic environment obstacle avoidance. [PITH_FULL_IMAGE:figures/full_fig_p007_8.png] view at source ↗

read the original 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/.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit. Tearing a paper down is the easy half of reading it; the pith above is the substance, this is the friction.

Desk Editor's Note private letter to a colleague

The paper vectorizes projection steps in manifold-constrained planning to claim 100-1000x CPU speedups and real-time humanoid demos, but the speedup numbers rest on unshown equivalence between serial and parallel constraint satisfaction.

read the letter

The main takeaway is that they rewrote the projection operations inside existing manifold planners to run under SIMD on ordinary CPUs, which they report turns planning times from tens of seconds down to real-time for whole-body humanoid tasks. They also put the method on a physical robot and released the code, which gives the work a practical anchor that many planning papers lack. That combination of targeted parallelization and an end-to-end hardware check is the part worth paying attention to if you need fast constrained trajectories without extra hardware. The engineering looks competent on the surface and the open-source link makes it possible to inspect the actual changes. The soft spot is exactly the one the stress-test flagged. Any reordering of floating-point work or shift in iteration counts inside the vectorized projections can change residuals or convergence behavior, so the trajectories might no longer lie on the manifold to the same precision as the original serial code. The abstract gives no side-by-side residual plots, success-rate tables on identical problem sets, or termination-criteria comparisons, which leaves the 100-1000x figures hard to interpret. If the vectorized version quietly accepts looser constraints to run faster, the performance claim and the “feasible for the first time” statement both weaken. This is aimed at robotics groups that already use projection-based planners and want to push them onto real hardware under tight timing. A reader who needs concrete implementation details and a working demo will get something usable from it. The paper is coherent enough and grounded enough in an external robot test that it should go to peer review rather than a desk reject, though the referees will need to press on the numerical-equivalence data.

Referee Report

3 major / 3 minor

Summary. The paper introduces a CPU SIMD-vectorized reformulation of projection operations within manifold-constrained motion planning. By restructuring projection steps for parallel execution, the method reports 100-1000x speed-ups over prior state-of-the-art planners, enabling real-time whole-body quasi-static planning for humanoid robots, with validation via simulation benchmarks and a hardware demonstration on a physical humanoid.

Significance. If the vectorized projections are shown to produce trajectories whose constraint residuals and convergence behavior are statistically indistinguishable from the serial baseline, the work would make real-time constrained planning practical for high-DoF systems, addressing a key bottleneck in dynamic robotics applications.

major comments (3)

[Experiments] Experiments section (benchmark tables and timing results): the reported 100-1000x speed-ups are measured on the vectorized implementation, but no side-by-side table compares maximum constraint residuals (e.g., manifold distance or collision penetration) or iteration counts between the serial reference and SIMD versions across the same problem instances; without this, it is impossible to confirm that speed-up does not come at the cost of relaxed termination or altered floating-point accumulation.
[Methods] Vectorized projection description (methods): the transformation of iterative projection into parallel SIMD structures is presented without an explicit proof or empirical test that the re-ordered operations preserve the original fixed-point convergence guarantees and exact manifold satisfaction to machine precision; any change in iteration count or accumulation order could allow trajectories that the serial planner would have rejected.
[Hardware Experiments] Hardware demonstration: success is asserted via real-robot execution, yet no quantitative metrics (e.g., percentage of plans with residual below 1e-6 or collision-check failure rate) are reported for the physical trials, leaving open whether the vectorized planner maintains the same safety guarantees as the baseline under real sensor noise and dynamics.

minor comments (3)

[Abstract] The abstract states 'up to 100-1000x' without specifying the exact baseline planner, problem dimensions, or hardware for each end of the range; a single clarifying sentence would improve reproducibility.
[Figures] Figure captions for timing and residual plots should explicitly state whether error bars represent standard deviation over 50 trials or worst-case values.
[Related Work] The related-work section omits recent CPU vectorization papers in sampling-based planning (e.g., works on AVX-accelerated RRT); adding 1-2 citations would better situate the contribution.

Simulated Author's Rebuttal

3 responses · 0 unresolved

We thank the referee for their thoughtful and constructive review. We address each major comment below, agreeing where revisions are needed to strengthen the evidence of equivalence between the serial and vectorized implementations.

read point-by-point responses

Referee: [Experiments] Experiments section (benchmark tables and timing results): the reported 100-1000x speed-ups are measured on the vectorized implementation, but no side-by-side table compares maximum constraint residuals (e.g., manifold distance or collision penetration) or iteration counts between the serial reference and SIMD versions across the same problem instances; without this, it is impossible to confirm that speed-up does not come at the cost of relaxed termination or altered floating-point accumulation.

Authors: We agree this comparison is essential for validating equivalence. The vectorized version uses identical termination criteria and the same projection operations, only restructured for SIMD. In the revised manuscript we will add a table reporting max constraint residuals and iteration counts for both versions on identical benchmark instances, confirming residuals remain at machine precision and iteration counts match. revision: yes
Referee: [Methods] Vectorized projection description (methods): the transformation of iterative projection into parallel SIMD structures is presented without an explicit proof or empirical test that the re-ordered operations preserve the original fixed-point convergence guarantees and exact manifold satisfaction to machine precision; any change in iteration count or accumulation order could allow trajectories that the serial planner would have rejected.

Authors: The reformulation applies SIMD only to independent sub-computations within each projection step while preserving the original iteration structure and accumulation order for dependent operations. We will add an empirical section with convergence plots and residual statistics across repeated runs on the same problems to demonstrate identical behavior to machine precision. A formal proof of fixed-point invariance under this specific reordering is not included and would require additional analysis beyond the current scope. revision: partial
Referee: [Hardware Experiments] Hardware demonstration: success is asserted via real-robot execution, yet no quantitative metrics (e.g., percentage of plans with residual below 1e-6 or collision-check failure rate) are reported for the physical trials, leaving open whether the vectorized planner maintains the same safety guarantees as the baseline under real sensor noise and dynamics.

Authors: We will expand the hardware section to report quantitative metrics from the physical trials, including the percentage of plans achieving residuals below 1e-6, collision-free execution rates, and any observed effects from sensor noise or dynamics. This will directly compare safety guarantees to the serial baseline. revision: yes

Circularity Check

0 steps flagged

No significant circularity; optimization is implementation-level with external validation

full rationale

The paper describes a CPU SIMD vectorization of existing projection-based manifold constraint satisfaction methods from prior literature. No load-bearing equations, parameters, or uniqueness claims reduce by construction to fitted inputs or self-citations. Performance claims rest on measured speed-ups and a physical robot demonstration, which serve as independent empirical checks rather than self-referential derivations. The contribution is algorithmic reimplementation for parallelism, not a new theoretical result that loops back on its own definitions.

Axiom & Free-Parameter Ledger

0 free parameters · 1 axioms · 0 invented entities

The work rests on standard assumptions from manifold optimization and robotics without introducing new free parameters, invented entities, or ad-hoc axioms beyond those already accepted in the field.

axioms (1)

domain assumption Constraint manifolds are smooth and the projection operator converges to a valid point on the manifold for feasible configurations.
Invoked implicitly when claiming that the vectorized projection still satisfies the original geometric constraints.

pith-pipeline@v0.9.0 · 5495 in / 1229 out tokens · 27013 ms · 2026-05-10T14:27:50.913199+00:00 · methodology

discussion (0)

Reference graph

Works this paper leans on

34 extracted references · 4 canonical work pages · 1 internal anchor

[1]

cuRobo: Parallelized collision-free robot motion generation

B. Sundaralingam, S. K. S. Hari, A. Fishman, C. Garrett, K. Van Wyk, V . Blukis, A. Millane, H. Oleynikova, A. Handa, F. Ramos, et al. “cuRobo: Parallelized collision-free robot motion generation”. In: IEEE Int. Conf. Robot. Autom. 2023, pp. 8112–8119

2023
[2]

GATO: GPU-Accelerated and Batched Trajectory Optimization for Scalable Edge Model Predictive Control

A. Du, E. Adabag, G. Bravo, and B. Plancher. “GATO: GPU- accelerated and batched trajectory optimization for scalable edge model predictive control”. In: arXiv preprint arXiv:2510.07625 (2025)

work page internal anchor Pith review arXiv 2025
[3]

Differentiable Particle Optimization for Fast Sequential Manipulation

L. Chen, S. R. Iyer, and Z. Kingston. “Differentiable Particle Optimization for Fast Sequential Manipulation”. In: arXiv preprint arXiv:2510.07674 (2025)

work page arXiv 2025
[4]

Motions in Mi- croseconds via Vectorized Sampling-Based Planning

W. Thomason, Z. Kingston, and L. E. Kavraki. “Motions in Mi- croseconds via Vectorized Sampling-Based Planning”. In: IEEE Int. Conf. Robot. Autom. 2024

2024
[5]

Task space regions: A framework for pose-constrained manipulation planning

D. Berenson, S. Srinivasa, and J. Kuffner. “Task space regions: A framework for pose-constrained manipulation planning”. In: Int. J. of Robotics Research 30.12 (2011), pp. 1435–1460

2011
[6]

Proba- bilistic roadmaps for path planning in high-dimensional configuration spaces

L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars. “Proba- bilistic roadmaps for path planning in high-dimensional configuration spaces”. In: IEEE Trans. Robot. Autom. 12.4 (1996), pp. 566–580

1996
[7]

RRT-connect: An efficient approach to single-query path planning

J. Kuffner and S. LaValle. “RRT-connect: An efficient approach to single-query path planning”. In: IEEE Int. Conf. Robot. Autom. V ol. 2. 2000, 995–1001 vol.2

2000
[8]

Path Planning Under Kinematic Con- straints by Rapidly Exploring Manifolds

L. Jaillet and J. M. Porta. “Path Planning Under Kinematic Con- straints by Rapidly Exploring Manifolds”. In: IEEE Trans. Robot. 29.1 (2013), pp. 105–117

2013
[9]

Sampling-Based Methods for Motion Planning with Constraints

Z. Kingston, M. Moll, and L. E. Kavraki. “Sampling-Based Methods for Motion Planning with Constraints”. In: Annual Review of Control, Robotics, and Autonomous Systems 1.1 (2018), pp. 159–185

2018
[10]

Neural manip- ulation planning on constraint manifolds

A. H. Qureshi, J. Dong, A. Choe, and M. C. Yip. “Neural manip- ulation planning on constraint manifolds”. In: IEEE Robot. Autom. Lett. 5.4 (2020), pp. 6089–6096

2020
[11]

Physics-informed neural motion planning on constraint manifolds

R. Ni and A. H. Qureshi. “Physics-informed neural motion planning on constraint manifolds”. In: IEEE Int. Conf. Robot. Autom. 2024, pp. 12179–12185

2024
[12]

Inverse kinematics-based mo- tion planning for dual-arm robot with orientation constraints

J. Wang, S. Liu, B. Zhang, and C. Yu. “Inverse kinematics-based mo- tion planning for dual-arm robot with orientation constraints”. In: Int. J. of Advanced Robotic Systems 16.2 (2019), p. 1729881419836858

2019
[13]

Constrained bimanual planning with analytic inverse kinematics

T. Cohn, S. Shaw, M. Simchowitz, and R. Tedrake. “Constrained bimanual planning with analytic inverse kinematics”. In: IEEE Int. Conf. Robot. Autom. 2024, pp. 6935–6942

2024
[14]

Motion planning around obstacles with convex optimization

T. Marcucci, M. Petersen, D. V on Wrangel, and R. Tedrake. “Motion planning around obstacles with convex optimization”. In: Science Robotics 8.84 (2023)

2023
[15]

C-FOREST: Parallel Shortest Path Planning With Superlinear Speedup

M. Otte and N. Correll. “C-FOREST: Parallel Shortest Path Planning With Superlinear Speedup”. In: IEEE Trans. Robot. 29.3 (2013), pp. 798–806

2013
[16]

Parallel sampling-based motion planning with superlinear speedup

J. Ichnowski and R. Alterovitz. “Parallel sampling-based motion planning with superlinear speedup”. In: IEEE/RSJ Int. Conf. Intell. Robot. Syst. 2012, pp. 1206–1212

2012
[17]

pRRTC: GPU-Parallel RRT-Connect for Fast, Consistent, and Low-Cost Mo- tion Planning

C. H. Huang, P. Jadhav, B. Plancher, and Z. Kingston. “pRRTC: GPU-Parallel RRT-Connect for Fast, Consistent, and Low-Cost Mo- tion Planning”. In: arXiv preprint arXiv:2503.06757 (2025)

work page arXiv 2025
[18]

Motion planning with sequential convex optimization and convex collision checking

J. Schulman, Y . Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel. “Motion planning with sequential convex optimization and convex collision checking”. In: Int. J. of Robotics Research 33.9 (2014), pp. 1251–1270

2014
[19]

CHOMP: Gradient optimization techniques for efficient motion planning

N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa. “CHOMP: Gradient optimization techniques for efficient motion planning”. In: IEEE Int. Conf. Robot. Autom. 2009, pp. 489–494

2009
[20]

cpRRTC: GPU-Parallel RRT- Connect for Constrained Motion Planning

J. Hu, J. Wang, and H. Christensen. “cpRRTC: GPU-Parallel RRT- Connect for Constrained Motion Planning”. In: arXiv preprint arXiv:2505.06791 (2025)

work page arXiv 2025
[21]

Synthesis of whole-body behaviors through hierarchical control of behavioral primitives

L. Sentis and O. Khatib. “Synthesis of whole-body behaviors through hierarchical control of behavioral primitives”. In: Int. J. of Humanoid Robotics 2.04 (2005), pp. 505–518

2005
[22]

Constellation-An algorithm for finding robot configurations that satisfy multiple constraints

P. Kaiser, D. Berenson, N. Vahrenkamp, T. Asfour, R. Dillmann, and S. Srinivasa. “Constellation-An algorithm for finding robot configurations that satisfy multiple constraints”. In: IEEE Int. Conf. Robot. Autom. 2012, pp. 436–443

2012
[23]

Con- strained nonlinear kaczmarz projection on intersections of manifolds for coordinated multi-robot mobile manipulation

A. Agrawal, P. Mayer, Z. Kingston, and G. A. Hollinger. “Con- strained nonlinear kaczmarz projection on intersections of manifolds for coordinated multi-robot mobile manipulation”. In: IEEE Int. Conf. Robot. Autom. 2025, pp. 7726–7732

2025
[24]

Learning constrained distributions of robot configurations with generative adversarial network

T. S. Lembono, E. Pignat, J. Jankowski, and S. Calinon. “Learning constrained distributions of robot configurations with generative adversarial network”. In: IEEE Robot. Autom. Lett. 6.2 (2021), pp. 4233–4240

2021
[25]

No falls, no resets: Reliable humanoid behavior in the DARPA robotics challenge

C. G. Atkeson, B. P. W. Babu, N. Banerjee, D. Berenson, C. P. Bove, X. Cui, M. DeDonato, R. Du, S. Feng, P. Franklin, et al. “No falls, no resets: Reliable humanoid behavior in the DARPA robotics challenge”. In: IEEE-RAS Int. Conf. on Humanoid Robots . 2015, pp. 623–630

2015
[26]

Adaptive tuning of the sampling domain for dynamic-domain RRTs

L. Jaillet, A. Yershova, S. M. La Valle, and T. Siméon. “Adaptive tuning of the sampling domain for dynamic-domain RRTs”. In: IEEE/RSJ Int. Conf. Intell. Robot. Syst. 2005, pp. 2851–2856

2005
[27]

J. J. Kuffner and S. M. LaValle. An efficient approach to path plan- ning using balanced bidirectional RRT search . Tech. rep. Robotics Institute, Carnegie Mellon University, 2005

2005
[28]

The Pinocchio C++ library – A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives

J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard. “The Pinocchio C++ library – A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives”. In: Int. Symp. on Syst. Integration. 2019

2019
[29]

B. Bell. CppAD: a package for C++ algorithmic differentiation . 2022

2022
[30]

Dahlquist and Å

G. Dahlquist and Å. Björck. Numerical methods . Courier Corpora- tion, 2003

2003
[31]

The foundations of set theoretic estimation

P. Combettes. “The foundations of set theoretic estimation”. In: Proceedings of the IEEE 81.2 (1993), pp. 182–208

1993
[32]

The Open Motion Planning Library

I. A. Sucan, M. Moll, and L. E. Kavraki. “The Open Motion Planning Library”. In: IEEE Robotics & Automation Magazine 19.4 (2012), pp. 72–82

2012
[33]

Exploring implicit spaces for constrained sampling-based planning

Z. Kingston, M. Moll, and L. E. Kavraki. “Exploring implicit spaces for constrained sampling-based planning”. In: Int. J. of Robotics Research 38.10-11 (2019), pp. 1151–1178

2019
[34]

Zero dynamics, pendulum models, and angular momentum in feedback control of bipedal locomotion

Y . Gong and J. W. Grizzle. “Zero dynamics, pendulum models, and angular momentum in feedback control of bipedal locomotion”. In: J. of Dynamic Systems, Measurement, and Control 144.12 (2022), p. 121006

2022