search

KavrakiLab / vamp

SIMD-Accelerated Sampling-based Motion Planning
Other
164 stars 22 forks source link
motion-planning robotics robotics-library simd

readme

🧛 Vector-Accelerated Motion Planning (VAMP)

arXiv VAMP arXiv CAPT Build Check Format Check

This repository hosts the code for the ICRA 2024 paper “Motions in Microseconds via Vectorized Sampling-Based Planning” as well as an implementation of the Collision-Affording Point Tree (CAPT) from the forthcoming RSS 2024 paper “Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking”.

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,
  title = {Motions in Microseconds via Vectorized Sampling-Based Planning},
  author = {Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.},
  booktitle = {IEEE International Conference on Robotics and Automation},
  date = {2024},
  url = {http://arxiv.org/abs/2309.14545},
}

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

@InProceedings{capt_2024,
  title = {Collision-Affording Point Trees: {SIMD}-Amenable Nearest Neighbors for Fast Collision Checking},
  author = {Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},
  booktitle = {Robotics: Science and Systems},
  date = {2024},
  url = {http://arxiv.org/abs/2406.02807},
  note = {To Appear.}
}

Building and Installing

VAMP requires the following system dependencies:

VAMP fetches the following external dependencies via CPM:

Download the code and submodules:

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 -B build -DCMAKE_BUILD_TYPE=Release .
cmake --build build

Please see CMakeLists.txt for further build configuration options.

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

Each robot in VAMP is provided as a Python submodule (e.g., vamp.panda, vamp.fetch) and supports the following functions:

For the flying sphere in $\mathbb{R}^3$, additional operations are available to set the domain of the sphere and the radius:

Supported RNG

We ship implementations of the following pseudorandom number generators (PRNGs):

Supported Planners

We currently ship two planners:

Note that these planners support planning to a set of goals, not just a single goal.

We also ship a number of heuristic simplification routines:

Planner Configuration and Hyperparameters

We provide a helper function vamp.configure_robot_and_planner_with_kwargs(robot, planner, **kwargs) to help configure all the planner and simplification settings that are available. Scripts that use this helper (sphere_cage_example.py, evaluate_mbm.py, visualize_mbm.py) provide the following arguments:

Each planner supports a number of settings. Both support the following:

For rrtc:

For prm, the settings must be configured with a neighbor parameter structure, e.g.:

robot_module = vamp.panda # or other robot submodule
prmstar_params = vamp.PRMNeighborParams(robot_module.dimension(), robot_module.space_measure())
prm_settings = vamp.PRMSettings(prmstar_params)

This is handled by default in the configuration function.

For simplification:

Environment Representation

VAMP currently supports collision checking against primitive models of the environment and pointclouds via CAPTs (see planned features for forthcoming extensions to meshes, etc.). Environments (vamp.Environment) can be constructed by adding objects (add_sphere(vamp.Sphere(...)), etc.). These objects can be created with the following:

Some robots (currently, the UR5, Panda, and Fetch) support attaching custom geometry (a collection of spheres) to the end-effector via vamp.Attachment(relative_position, relative_quaternion_xyzw). Spheres can be added (in the attachment's frame) with add_sphere(...). The attachment can be added to the environment with vamp.Environment.attach(...), and removed with vamp.Environment.detach(). An example use of attachments with the Panda arm is available in scripts/attachments.py.

Code Overview

The code lives in the src folder, split into impl (the C++ core) and vamp (the Python interface). Scripts live in the scripts/ folder; see the README in that directory for more information.

Inside impl/vamp, the code is divided into the following directories:

Planned Features

References