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

TL;DR

This paper introduces a SIMD-accelerated motion planner that significantly speeds up manifold-constrained planning, enabling real-time whole-body control for humanoid robots.

Contribution

It presents a novel parallelization approach for projection-based constraint satisfaction, achieving up to 1000x speed-ups over existing methods.

Findings

Achieves up to 1000x speed-up over state-of-the-art methods.

Enables real-time whole-body motion planning for humanoid robots.

Demonstrates successful real-time control on a real humanoid robot.

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/.