🧛 Vector-Accelerated Motion Planning (VAMP)

For a full demonstration of VAMP running in real-time, see this video.

This repository hosts the code for:

• the ICRA 2024 paper “Motions in Microseconds via Vectorized Sampling-Based Planning”,
• an implementation of the Collision-Affording Point Tree (CAPT) from the RSS 2024 paper “Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking”,
• an implementation of the Fully Connected Informed Trees (FCIT*) algorithm from the ICRA 2025 paper “Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees (FCIT*)”.
• an implementation of the Asymptotically Optimal RRT-Connect (AORRTC) algorithm from the RA-L submission “AORRTC: Almost-Surely Asymptotically Optimal Planning with RRT-Connect”.

TL;DR: By exploiting ubiquitous CPU SIMD instructions to accelerate collision checking and forward kinematics (FK), vamp's RRT-Connect [1] solves problems for the Franka Emika Panda from the MotionBenchMaker dataset [3] at a median speed of 35 microseconds (on one core of a consumer desktop PC). This approach to hardware-accelerated parallel sampling-based motion planning extends to other planning algorithms without modification (e.g., PRM [2]) and also works on low-power systems (e.g., an ARM-based OrangePi). We also accelerate collision checking against pointclouds with a novel spatial data structure, the Collision-Affording Point Tree (CAPT), which has an average query time of less than 10 nanoseconds on 3D scenes composed of thousands of points.

If you found this research useful for your own work, please use the following citation:

@InProceedings{vamp_2024,
  author = {Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.},
  title = {Motions in Microseconds via Vectorized Sampling-Based Planning},
  booktitle = {IEEE International Conference on Robotics and Automation},
  pages = {8749--8756},
  url = {http://arxiv.org/abs/2309.14545},
  doi = {10.1109/ICRA57147.2024.10611190},
  date = {2024}
}

If you use CAPTs or the pointcloud collision checking components of this repository, please also use the following citation:

@InProceedings{capt_2024,
  author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},
  title = {Collision-Affording Point Trees: {SIMD}-Amenable Nearest Neighbors for Fast Collision Checking},
  booktitle = {Robotics: Science and Systems},
  url = {https://www.roboticsproceedings.org/rss20/p038.pdf},
  doi = {10.15607/RSS.2024.XX.038},
  date = {2024}
}

If you use FCIT*, please use the following citation:

@InProceedings{fcit_2025,
  author = {Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary  and Kavraki, Lydia E. and Gammell, Jonathan D.},
  title = {Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees ({FCIT*})},
  booktitle = {IEEE International Conference on Robotics and Automation},
  url = {https://arxiv.org/abs/2411.17902},
  date = {2025}
}

If you use AORRTC, please use the following citation:

@article{aorrtc_2025,
  author = {Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary and Gammell, Jonathan D.},
  title = {{AORRTC}: Almost-surely asymptotically optimal planning with {RRT-Connect}},
  journal = {IEEE Robotics and Automation Letters},
  url = {https://arxiv.org/abs/2505.10542},
  year = {2025},
  note = {Under Review}
}

Installation

You can simply download the latest release of VAMP from PyPI with:

pip install vamp-planner

[!IMPORTANT]
VAMP comes with precompiled robots! If you want to add your own, use cricket and follow the instructions there.

VAMP requires the following system dependencies:

• CMake version 3.16 or greater.
• GCC 8+ or Clang 10+, along with the C++ standard library. To install GCC on Ubuntu, sudo apt install build-essential. To install Clang and its C++ standard library implementation on Ubuntu 22.04, sudo apt install clang libstdc++6
• Python development headers for generating Python bindings. We support Python 3.8 and above. To install on Ubuntu 22.04, sudo apt install python3-dev.
• Eigen3 for some vector/matrix operations. To install on Ubuntu 22.04, sudo apt install libeigen3-dev. Note that we require at least Eigen 3.4, which is not available by default on Ubuntu 20.04.

Installation from Source

VAMP fetches the following external dependencies via CPM:

• nanobind: for Python bindings
• nigh: a fork of the original nigh [4] to better use our vector types
• pdqsort: for fast sorting
• SIMDxorshift: alternative fast random numbers for x86 machines

Download the code:

git clone git@github.com:KavrakiLab/vamp.git

Python

For use through Python, install with pip:

cd vamp
pip install .

If you want to install all Python dependencies to run the examples, specify those optional dependencies:

pip install .[examples,heightmaps]

If you have installed the examples dependencies, test your installation by running:

python scripts/sphere_cage_example.py --visualize

Which will benchmark a simple scenario of the Franka Emika Panda in a cage of spheres and visualize one of the results. See the README in the scripts directory for more details.

Incremental Rebuilds

Rather than building the entire library from scratch each time, nanobind supports incremental rebuilds:

cd vamp
pip install --no-build-isolation -Ceditable.rebuild=true -ve .

C++

If you wish to extend vamp via C++, please build directly with CMake, e.g.:

cd vamp
cmake -Bbuild -DCMAKE_BUILD_TYPE=Release .
cmake --build build

Please see CMakeLists.txt for further build configuration options.

Architecture-Specific Build Options

By default, VAMP builds with -march=native for optimal performance on the build machine. For builds targeting different hardware (e.g., Docker containers), you can override the architecture flags:

cmake -Bbuild -DCMAKE_BUILD_TYPE=Release -DVAMP_ARCH="-march=x86-64-v3 -mavx2" .

Example options:

• -march=x86-64-v3 -mavx2: Supports most modern x86_64 systems (2013+), includes BMI2 instructions required by VAMP
• -march=native -mavx2: Default setting, optimizes for build machine's specific CPU

Docker

We provide example dockerfiles in docker/ that show installation on Ubuntu 20.04, 22.04, and 24.04.

Conda/Mamba

Installation in Conda/Mamba environments is supported. See the environment.yaml file for a basic environment, and see docker/ubuntu2204-conda.dockerfile for an example installation.

Supported Platforms

We currently support x86 CPUs (e.g., Intel, AMD) with the AVX2 vector instruction set and ARM CPUs (e.g., Raspberry Pi, Mac M1) with NEON. Please see the docker/ folder for reference installation procedures.

Using Clang instead of GCC

You can force the use of Clang instead of GCC for compiling VAMP by uncommenting the line at the bottom of the pyproject.toml (or setting the corresponding CMake variable for C++ builds):

[tool.scikit-build.cmake.define]
VAMP_LTO = "ON"
VAMP_FORCE_CLANG = "ON"

This may have performance implications for some systems (positive or negative). We recommend trying both compilers to see which works best for your particular setup.

Supported Robots

We ship code to do planning for a sphere in $\mathbb{R}^3$ and the UR5, Panda, Fetch, and Baxter models as found in robowflex_resources [5], as used in the MotionBenchMaker (MBM) [3] dataset. Resources for each robot (URDF, SRDF, meshes, etc.) are all provided in the resources/ directory under each robot's name. See the README for more information on the robot models.

The MBM problems for each robot are compressed in problems.tar.bz2. For the UR5, Panda, and Fetch, these problems are the table_pick, table_under_pick, box, bookshelf_small, bookshelf_tall, `bo