🧛 Vector-Accelerated Motion Planning (VAMP)

For a full demonstration of VAMP running in real-time, see this video. You can also play with an interactive demonstration here!.

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.

WebAssembly (WASM)

VAMP can be compiled to WebAssembly for use in web browsers via Emscripten. The WASM build uses 128-bit SIMD instructions.

Building WASM Modules

First, install Emscripten. Then build the WASM modules:

cd vamp
emcmake cmake -Bbuild-wasm -DCMAKE_BUILD_TYPE=Release -DVAMP_BUILD_WASM=ON -DVAMP_BUILD_PYTHON_BINDINGS=OFF
cmake --build build-wasm

This produces {robot}_wasm.js and {robot}_wasm.wasm files in build-wasm/wasm/ for each robot (by default: panda, fetch, ur5).

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, bookshelf_thin, and cage scenarios, each with 100 problems. For the Baxter, these problems are bookshelf_tall_both_arms_easy, bookshelf_tall_both_arms_medium, and bookshelf_tall_both_arms_hard scenarios, each with 600 problems (note that the difficulty modifier refers to the amount of variation in the scene, not difficulty of finding a problem solution). These problems can be decompressed into a convenient pickle and JSON format with the script resources/problem_tar_to_pkl_json.py, after VAMP has been installed:

# choose robot name from {ur5, panda, fetch, baxter}
python resources/problem_tar_to_pkl_json.py --robot panda

This only needs to be run once.

Custom Robots

VAMP uses a tracing compilation step to generate code for the SIMD raked collision check. This compiler is available in the cricket repository. There are instructions in cricket's readme for how to setup a new robot. You will also need to come up with a spherical decomposition of the robot's collision geometry. This can be done automatically with the tool foam.

[!WARNING]
There may be some tuning of the spherization of the robot necessary to get everything to work! Start with a finer approximation of the robot and work up from there.

Robot-Specific Functions

Each robot in VAMP is provided as a Python submodule (e.g., vamp.panda, vamp.fetch) and supports the following functions: - rrtc: RRT-Connect. See Supported Planners. - prm: PRM. See Supported Planners. - fcit: FCIT*. See Supported Planners. - aorrtc: AORRTC. See Supported Planners. - roadmap: returns the constructed roadmap generated by PRM. - simplify: simplifies a planned path. - validate: checks if a standalone configuration in collision. - debug: returns information on what spheres of the robot are colliding with each other and the environment. - fk: performs FK to compute the locations of all robot collision spheres. - eefk: compute the end-effector transform for a given configuration. Used by attachments. - filter_self_from_pointcloud: removes points in the pointcloud that are currently in collision with the robot (i.e., points which probably belong to the robot, if the robot is in a known valid configuration).

For the flying sphere in $\mathbb{R}^3$, additional operations are available to set the domain of the sphere and the radius: - vamp.sphere.set_lows() and vamp.sphere.set_highs() to set bounding box of space - vamp.sphere.set_radius() to set the sphere's radius

Supported RNG

We ship implementations of the following pseudorandom number generators (PRNGs): - halton: An implementation of a multi-dimensional Halton sequence [12-13]. - xorshift: A SIMD-accelerated implementation of an XOR shift generator, only available on x86 machines. Uses the SIMDxorshift library.

Supported Planners

We currently ship four planner