VAMP Integration: VAMPStateSpace for SIMD-Accelerated Motion Planning

Overview

This PR integratesVAMP (Vector-Accelerated Motion Planning) into OMPL through a new state space,ompl::geometric::VAMPStateSpace. VAMP leverages CPU SIMD instructions (AVX2/NEON) to perform collision checking and forward kinematics orders of magnitude faster than traditional methods, achieving planning speeds in themicroseconds range for manipulator robots.

Note: This PR replaces the previousdemos/Vamp/ directory with a cleaner state space-based design.

What is VAMP?

VAMP (Vector-Accelerated Motion Planning) usesvectorized instructions to process multiple robot configurations simultaneously, enabling massively accelerated collision detection. By exploiting ubiquitous CPU SIMD capabilities, VAMP's RRT-Connect achieves:

• Median planning time: 35 microseconds (Franka Panda on MotionBenchMaker dataset)
• 100-1000x speedup vs. traditional collision checking
• Single-core performance on consumer desktop CPUs

Implementation

VAMPStateSpace

The newompl::geometric::VAMPStateSpace template class extendsRealVectorStateSpace to provide SIMD-accelerated collision detection while remaining fully compatible with all OMPL geometric planners.

Usage:

// Create VAMP collision environmentvamp::collision::Environment<float> environment;environment.spheres.emplace_back(0.3,0.0,0.5,0.2);// x, y, z, radius// Create VAMP state space for Panda robotauto space = std::make_shared<ompl::geometric::VAMPStateSpace<vamp::robots::Panda>>(environment);// Use with standard OMPL SimpleSetupompl::geometric::SimpleSetupss(space);auto si = ss.getSpaceInformation();ss.setStateValidityChecker(space->allocDefaultStateValidityChecker(si));si->setMotionValidator(space->allocDefaultMotionValidator(si));// Standard OMPL workflowss.setStartAndGoalStates(start, goal);ss.setPlanner(std::make_shared<RRTConnect>(si));ss.solve(5.0);

Key Features:

• Automatic Configuration: Joint limits set automatically from robot definitions
• Custom Validators: SIMD-acceleratedStateValidityChecker andMotionValidator
• Multiple Robots: Pre-configured support for Panda (Franka Emika), UR5, Fetch, and Baxter
• Flexible Environments: Supports primitive obstacles (spheres, boxes, capsules) and point cloud environments via CAPT (Collision-Affording Point Tree)
• Full OMPL Compatibility: Works seamlessly with all geometric planners, SimpleSetup, and benchmarking tools

Changes

Added:

1. src/ompl/base/spaces/VAMPStateSpace.h (243 lines)

   • Template state space class for VAMP-enabled robots
   • CustomStateValidityChecker using vectorized collision detection
   • CustomMotionValidator using vectorized continuous collision checking
   • Automatic joint limit configuration from robot models

2. demos/Vamp.cpp (330 lines)

   • Clean single-file demonstration program
   • --simple mode: Basic planning with RRTConnect
   • --benchmark mode: Performance comparison across multiple planners (RRTConnect, RRTstar, BITstar, PRM)
   • Compatible withompl_benchmark_statistics.py and Planner Arena

3. tests/base/state_spaces.cpp (4 new tests)

   • State space creation and configuration validation
   • State validity checking tests
   • Motion validation tests
   • Multi-robot support verification

4. Documentation

   • Updated release notes for OMPL 2.0.0beta
   • Added VAMP demo todemos.md
   • Added VAMPStateSpace tospaces.md
   • Comprehensive Doxygen documentation in header file

Removed:

• demos/Vamp/ directory (old integration approach)
• Associated VAMP documentation images

Modified:

• CMake configuration withOMPL_BUILD_VAMP option (enabled by default)
• GitHub Actions workflows for VAMP dependency handling

Demo Usage

# Build with VAMP support (enabled by default)cmake .. -DOMPL_BUILD_VAMP=ONmake# Run simple planning demonstration./demos/demo_Vamp --simple# Run benchmark comparison./demos/demo_Vamp --benchmark# Process benchmark resultsscripts/ompl_benchmark_statistics.py vamp_ompl_benchmark.log -d vamp_benchmark.db# Visualize at plannerarena.org or generate plotsscripts/ompl_benchmark_statistics.py -d vamp_benchmark.db -p plots.pdf

Architecture

VAMPStateSpace integrates VAMP into OMPL through three key components:

1. State Space: ExtendsRealVectorStateSpace with robot-specific configuration
2. State Validity Checker: Vectorized collision detection for individual states
3. Motion Validator: Vectorized continuous collision detection for path segments

This design allowsany OMPL geometric planner to benefit from VAMP's acceleration without modification—simply swapRealVectorStateSpace forVAMPStateSpace.

Supported Robots

• Panda (Franka Emika) - 7 DOF
• UR5 (Universal Robots) - 6 DOF
• Fetch - 7 DOF
• Baxter - 7 DOF (per arm)

Testing

# Run all tests./test/test_base_state_spaces# Integration tests verify:# - State space configuration and bounds# - VAMP state validity checking# - VAMP motion validation# - Multi-robot compatibility

References

• Wil Thomason, Zachary Kingston, and Lydia E. Kavraki,"Motions in Microseconds via Vectorized Sampling-Based Planning,"ICRA 2024
• VAMP library:https://github.com/KavrakiLab/vamp
• Collision-Affording Point Tree:"SIMD-Amenable Nearest Neighbors for Fast Collision Checking,"RSS 2024

This integration brings state-of-the-art SIMD-accelerated motion planning to OMPL through a clean, extensible state space design that works seamlessly with the existing OMPL ecosystem.