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.
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.
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 → M
M → B
B → T
IK-BiRRT
96.74 ± 48.48
90.11 ± 21.70
121.90 ± 53.61
(3.69 ± 0.63)
(5.64 ± 0.96)
(6.26 ± 1.09)
IK-GCS
63.47 ± 1.95
44.18 ± 2.74
77.87 ± 1.19
( 2.52 ± 0.0 )
( 4.73 ± 0.0 )
( 5.82 ± 0.0 )
McVAMP
2.11 ± 1.16
3.53 ± 2.44
2.36 ± 1.63
(1-step)
(7.72 ± 1.50)
(6.08 ± 1.51)
(8.93 ± 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) CB , the bimanual fixed pose constraint, (ii) CCL , the closed linkage constraint, (iii) CCOM , the center of mass constraint, and (iv) CF , the fixed feet placement constraint.
Motion phases
S → T
T → B
B → M
M → T
Tracking error (deg)
Left Knee Joint
1.70 ± 1.57
0.64 ± 0.38
1.28 ± 0.34
1.00 ± 0.50
Left Tarsus Joint
4.37 ± 2.26
4.80 ± 1.82
8.63 ± 1.73
6.38 ± 1.31
Right Knee Joint
3.81 ± 1.78
2.57 ± 1.05
5.61 ± 0.71
4.51 ± 0.63
Right Tarsus Joint
3.51 ± 1.73
5.15 ± 1.93
7.74 ± 1.93
5.85 ± 1.89
Planning Time (ms)
4.93 ± 0.12
10.11 ± 1.65
13.42 ± 3.68
16.28 ± 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 ± 1 standard deviation.
Figure 8: Experimental results for dynamic environment obstacle avoidance.
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.
Gahyun Oh, Junho An, Suhyun Jeon +1
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.
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.
Shrutheesh R. Iyer, Thomas Cohn, Zachary Kingston
Department of Computer Science, Purdue University · Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology
Multi-robot-arm motion planning is a key challenge in deploying multiple manipulators for industrial tasks such as manufacturing. Existing search-based and sampling-based solvers often require significant computation time to produce collision-free, high-quality motions suitable for safe real-world execution. In this work, we introduce a new suite of multi-robot-arm motion planners capable of near real-time motion generation, combining classical planning algorithms with state-of-the-art vectorized collision-checking techniques. Based on CPU SIMD instructions, our new planners accelerate their primary bottleneck, collision checking, and achieve up to two orders of magnitude speedup in both motion planning and execution postprocessing for multi-arm manipulation tasks. We also release our implementation to lower the barrier for research and development of multi-robot-arm planning and manipulation problems. Code is available at https://vamp-mr.github.io/vamp-mr
Philip Huang, Chenrui Gao, Jiaoyang Li
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA · University of Michigan, Ann Arbor