Initial CAPT release (#11)

* Initial CAPT release Co-authored-by: Clayton Ramsey <[email protected]> Co-authored-by: Wil Thomason <[email protected]> * fix: formatting of ur5 fk * fix: environment formatting --------- Co-authored-by: Clayton Ramsey <[email protected]> Co-authored-by: Wil Thomason <[email protected]>
KavrakiLab · Jun 6, 2024 · 0cc51b8 · 0cc51b8
1 parent 8a456e1
commit 0cc51b8
Show file tree

Hide file tree

Showing 25 changed files with 1,535 additions and 50 deletions.
diff --git a/.gitignore b/.gitignore
@@ -18,3 +18,4 @@ venv/
 /resources/panda/*.json
 /resources/panda/*.pkl
 src/vamp/_core/*.pyi
+/.mypy_cache/
diff --git a/CITATION.bib b/CITATION.bib
@@ -4,5 +4,4 @@ @InProceedings{vamp
   booktitle = {IEEE International Conference on Robotics and Automation},
   date = {2024},
   url = {http://arxiv.org/abs/2309.14545},
-  note = {To Appear.}
 }
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -22,7 +22,7 @@ endif()
 
 project(
     vamp
-    VERSION 0.1
+    VERSION 0.2
     LANGUAGES C CXX
 )
 

diff --git a/README.md b/README.md
@@ -1,13 +1,15 @@
 # 🧛 Vector-Accelerated Motion Planning (VAMP)
 
-[![arXiv](https://img.shields.io/badge/arXiv-2309.14545-b31b1b.svg)](https://arxiv.org/abs/2309.14545) 
+[![arXiv VAMP](https://img.shields.io/badge/arXiv-2309.14545-b31b1b.svg)](https://arxiv.org/abs/2309.14545)
+[![arXiv CAPT](https://img.shields.io/badge/arXiv-2406.02807-b31b1b.svg)](https://arxiv.org/abs/2406.02807)
 [![Build Check](https://github.com/KavrakiLab/vamp/actions/workflows/build.yml/badge.svg)](https://github.com/KavrakiLab/vamp/actions/workflows/build.yml)
 [![Format Check](https://github.com/KavrakiLab/vamp/actions/workflows/format.yml/badge.svg)](https://github.com/KavrakiLab/vamp/actions/workflows/format.yml)
 
-This repository hosts the code for the forthcoming ICRA 2024 paper “Motions in Microseconds via Vectorized Sampling-Based Planning” (https://arxiv.org/abs/2309.14545).
+This repository hosts the code for the ICRA 2024 paper “Motions in Microseconds via Vectorized Sampling-Based Planning” (https://arxiv.org/abs/2309.14545) 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” (http://arxiv.org/abs/2406.02807).
 
 **TL;DR**: By exploiting ubiquitous [CPU SIMD instructions](https://en.wikipedia.org/wiki/Single_instruction,_multiple_data) to accelerate collision checking and forward kinematics (FK), `vamp`'s RRT-Connect [[1]](#1) solves problems for the Franka Emika Panda from the MotionBenchMaker dataset [[3]](#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]](#2)) and also works on low-power systems (e.g., an ARM-based [OrangePi](http://www.orangepi.org/)).
+We also make collision checking against pointclouds fast 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:
 ```bibtex
@@ -17,6 +19,17 @@ If you found this research useful for your own work, please use the following ci
   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 use the following citation:
+```bibtex
+@InProceedings{capt,
+  title = {Collision-Affording Point Trees: {SIMD}-Amenable Nearest Neighbors for Fast Collision Checking},
+  author = {Ramsey, Clayton W. and Kingston, Zachary and Tomason, Wil and Kavraki, Lydia E.},
+  booktitle = {Robotics: Science and Systems},
+  date = {2024},
+  url = {http://arxiv.org/abs/2406.02807},
   note = {To Appear.}
 }
 ```
@@ -114,7 +127,9 @@ Each robot in VAMP is provided as a Python submodule (e.g., `vamp.panda`, `vamp.
 - `roadmap`: returns the constructed roadmap generated by PRM
 - `simplify`: simplifies a planned path
 - `validate`: checks if a standalone configuration in collision
+- `sphere_validity`: returns a list of all objects each of the robot's spheres are in collision with
 - `fk`: performs FK to compute the locations of all robot collision spheres
+- `filter_from_pointcloud`: removes points in the pointcloud that are currently in collision with the robot (i.e., points which probably belong to the robot)
 
 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
@@ -182,14 +197,14 @@ See `simplify_settings.hh` for more information.
 
 ## Environment Representation
 
-VAMP currently supports collision checking against primitive models of the environment (see
-[planned features](#Planned-Features) for forthcoming extensions to meshes, etc.).
-Environments (`vamp.Environment`) can be constructed by adding primitives (`add_sphere(vamp.Sphere(...))`, etc.).
-These primitives can be created with the following:
+VAMP currently supports collision checking against primitive models of the environment and pointclouds via CAPTs (see [planned features](#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:
 - `vamp.Sphere(position, radius)`: a sphere with position and radius.
 - `vamp.Capsule(center, euler_xyz, radius, length)` and `vamp.Capsule(endpoint1, endpoint2, radius)`: a capsule in space, specified by either its frame, radius, and length or by the endpoints and radius.
 - `vamp.Cuboid(center, euler_xyz, half_extents)`: a cuboid specified by the frame and then half-extents (radii) along the X, Y, and Z axes in its local frame.
 - `vamp.Heightfield` via `vamp.make_heightfield` / `vamp.png_to_heightfield`: a heightfield specified by pixel intensity in an image file, scaled over specified dimensions.
+- Pointclouds via `add_pointcloud()` in `vamp.Environment`. This will construct a CAPT from the provided list of points, the minimum and maximum robot sphere radii, and radius for each point in the pointcloud.
 See the `src/impl/vamp/collision/` folder for more information.
 
 ## Code Overview
@@ -212,6 +227,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
 - `collision/`:
   Collision checking routines and environment description.
   Primitives are described in `shapes.hh`, the methods to create them in `factory.hh`, the environment in `environment.hh`, and collision checking of spheres against the environment in `validity.hh`.
+  CAPTs are implemented in `capt.hh`, with pointcloud filtering in `filter.hh`.
 
 - `planning/`:
   Planning and simplification routines.
@@ -232,7 +248,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
 - [ ] Planning subgroups
 - [ ] Object attachment at end-effector
 - [ ] Mesh collision checking
-- [ ] Pointcloud collision checking
+- [X] Pointcloud collision checking
 - [ ] Manifold-constrained planning
 - [ ] Time-optimal trajectory parameterization
 - and more...

diff --git a/cmake/CompilerSettings.cmake b/cmake/CompilerSettings.cmake
@@ -12,7 +12,7 @@ endif()
 set(VAMP_FAST_ARGS "-fno-math-errno -fno-signed-zeros -fno-trapping-math -fno-rounding-math -ffp-contract=fast")
 
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") # x86 supports additional flags
-	string(APPEND VAMP_FAST_ARGS " -ffinite-math-only -fassociative-math")
+	string(APPEND VAMP_FAST_ARGS " -fassociative-math")
 	if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") # Clang supports additional fine-grained flags over GCC
 		string(APPEND VAMP_FAST_ARGS " -fno-honor-infinities -fno-honor-nans")
 		if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 11.0.0) # clang 11 does not support -fapprox-func

diff --git a/cmake/Dependencies.cmake b/cmake/Dependencies.cmake
@@ -3,7 +3,7 @@ find_package(Python 3.8
   REQUIRED COMPONENTS Interpreter Development.Module
   OPTIONAL_COMPONENTS Development.SABIModule)
 
-CPMAddPackage("gh:wjakob/nanobind#03ebcb883a8658706bada56e7bc11faf2e2b3f67")
+CPMAddPackage("gh:wjakob/nanobind#4ed5fdf80de460edc416c0e948d6e6c60e61a02a")
 
 CPMAddPackage("gh:kavrakilab/nigh#97130999440647c204e0265d05a997dbd8da4e70")
 set(NIGH_INCLUDE_DIRS ${nigh_SOURCE_DIR}/src)

diff --git a/pyproject.toml b/pyproject.toml
@@ -1,16 +1,17 @@
 [build-system]
-requires = ["scikit-build-core >=0.4.3", "nanobind >=1.3.2", "cmake >=3.1", "typing_extensions"]
+requires = ["scikit-build-core >=0.4.3", "nanobind >=2.0.0", "cmake >=3.1", "typing_extensions"]
 build-backend = "scikit_build_core.build"
 
 [project]
 name = "vamp"
-version = "0.1"
+version = "0.2"
 description = "Vector-Accelerated Motion Planner"
 readme = "README.md"
 requires-python = ">=3.8"
 authors = [
     { name = "Zachary Kingston", email = "[email protected]" },
     { name = "Wil Thomason", email = "[email protected]" },
+    { name = "Clayton Ramsey", email = "[email protected]" }
 ]
 classifiers = [
     "License :: Polyform Noncommercial",

diff --git a/resources/README.md b/resources/README.md
@@ -76,6 +76,44 @@ Completed all problems in 137.891 milliseconds
 Total time including Python overhead: 236.387 milliseconds
 ```
 
+Using CAPTs for pointclouds:
+```
+|      |   Planning Time (μs) |   Simplification Time (μs) |   Total Time (μs) |   Planning Iters. |   Time per Iter. (μs) |
+|------|----------------------|----------------------------|-------------------|-------------------|-----------------------|
+| mean |              337.898 |                    202.762 |           541.141 |           1112.89 |              2.43163  |
+| std  |              995.4   |                    157.294 |          1058.5   |           3177.61 |              2.67853  |
+| min  |               15     |                      2     |            20     |              0    |              0        |
+| 25%  |               58     |                     76     |           164.75  |             42    |              0.604432 |
+| 50%  |               90     |                    166     |           271.5   |            162    |              1.728    |
+| 75%  |              182.25  |                    290.5   |           498.5   |            580.25 |              3.30235  |
+| 95%  |             1525.05  |                    516.3   |          1897.75  |           5448.05 |              6.88009  |
+| max  |            12450     |                    853     |         13044     |          37051    |             24.1276   |
+|      |    Initial Cost (L2) |       Simplified Cost (L2) |
+|------|----------------------|----------------------------|
+| mean |             11.2462  |                    6.68846 |
+| std  |              4.31888 |                    1.86498 |
+| min  |              1.86895 |                    1.86895 |
+| 25%  |              8.40302 |                    5.36698 |
+| 50%  |             10.1019  |                    6.51718 |
+| 75%  |             13.0541  |                    7.93681 |
+| 95%  |             19.1538  |                   10.2341  |
+| max  |             37.6167  |                   12.7789  |
+|      |     Filter Time (ms) |       CAPT Build Time (ms) |   Total Time (ms) |
+|------|----------------------|----------------------------|-------------------|
+| mean |              2.97212 |                    4.25712 |           7.77142 |
+| std  |              1.08287 |                    1.75468 |           2.85696 |
+| min  |              1.305   |                    1.623   |           3.066   |
+| 25%  |              2.40475 |                    2.6185  |           5.4575  |
+| 50%  |              2.5805  |                    4.226   |           7.0575  |
+| 75%  |              2.8385  |                    5.36225 |           9.5895  |
+| 95%  |              5.4488  |                    7.56915 |          13.1883  |
+| max  |              6.147   |                    8.675   |          19.828   |
+Solved / Valid / Total # Problems: 608 / 608 / 700
+Completed all problems in 329.014 milliseconds
+Total time including Python overhead: 37523.806 milliseconds
+```
+
+
 #### Panda
 ```
 |      |   Planning Time (μs) |   Simplification Time (μs) |   Total Time (μs) |   Planning Iters. |   Time per Iter. (μs) |
@@ -103,6 +141,43 @@ Completed all problems in 99.162 milliseconds
 Total time including Python overhead: 210.897 milliseconds
 ```
 
+Using CAPTs for pointclouds:
+```
+|      |   Planning Time (μs) |   Simplification Time (μs) |   Total Time (μs) |   Planning Iters. |   Time per Iter. (μs) |
+|------|----------------------|----------------------------|-------------------|-------------------|-----------------------|
+| mean |              206.778 |                    243.318 |           450.584 |           375.525 |              1.15073  |
+| std  |              296.366 |                    213.687 |           458.693 |          1781.27  |              1.68118  |
+| min  |               15     |                      2     |            18     |             0     |              0        |
+| 25%  |               72     |                     85     |           185     |            26     |              0.24737  |
+| 50%  |              111     |                    188     |           330     |            85     |              0.715909 |
+| 75%  |              207     |                    338.5   |           552.5   |           245.5   |              1.36029  |
+| 95%  |              711     |                    656.8   |          1244.8   |          1566.9   |              4.09809  |
+| max  |             2782     |                   1432     |          4183     |         42794     |             22.3584   |
+|      |    Initial Cost (L2) |       Simplified Cost (L2) |
+|------|----------------------|----------------------------|
+| mean |              8.70002 |                    5.16676 |
+| std  |              3.51816 |                    1.28799 |
+| min  |              2.78403 |                    2.78403 |
+| 25%  |              6.38199 |                    4.29836 |
+| 50%  |              7.76216 |                    4.90825 |
+| 75%  |             10.0274  |                    5.59457 |
+| 95%  |             16.2303  |                    8.00961 |
+| max  |             29.3161  |                   11.1757  |
+|      |     Filter Time (ms) |       CAPT Build Time (ms) |   Total Time (ms) |
+|------|----------------------|----------------------------|-------------------|
+| mean |              2.65961 |                    2.87958 |           5.99077 |
+| std  |              1.20675 |                    1.52154 |           2.65257 |
+| min  |              0.689   |                    0.445   |           1.195   |
+| 25%  |              1.981   |                    1.7385  |           4.179   |
+| 50%  |              2.303   |                    2.132   |           5.012   |
+| 75%  |              2.6725  |                    4.0525  |           7.1435  |
+| 95%  |              5.4955  |                    5.9582  |          11.8494  |
+| max  |              6.063   |                    6.988   |          13.244   |
+Solved / Valid / Total # Problems: 699 / 699 / 700
+Completed all problems in 314.958 milliseconds
+Total time including Python overhead: 41970.451 milliseconds
+```
+
 #### Fetch
 ```
 |      |   Planning Time (μs) |   Simplification Time (μs) |   Total Time (μs) |   Planning Iters. |   Time per Iter. (μs) |
@@ -130,6 +205,44 @@ Completed all problems in 2803.015 milliseconds
 Total time including Python overhead: 3152.705 milliseconds
 ```
 
+Using CAPTs for pointclouds:
+```
+
+|      |   Planning Time (μs) |   Simplification Time (μs) |   Total Time (μs) |   Planning Iters. |   Time per Iter. (μs) |
+|------|----------------------|----------------------------|-------------------|-------------------|-----------------------|
+| mean |             11121.4  |                    951.833 |          12073.7  |           18192.2 |              1.85053  |
+| std  |             31058.9  |                    710.608 |          31347.4  |           57625.7 |              2.3801   |
+| min  |                66    |                     55     |            225    |              60   |              0.297561 |
+| 25%  |               540.25 |                    495.5   |           1206.25 |             659   |              0.755285 |
+| 50%  |              2354    |                    777     |           3114    |            3997.5 |              1.09338  |
+| 75%  |             10157.2  |                   1175.25  |          11168.2  |           12895.5 |              2.12096  |
+| 95%  |             44671    |                   2285.6   |          47224.9  |           75164.7 |              5.13299  |
+| max  |            463080    |                   5526     |         465414    |          880485   |             37.1921   |
+|      |    Initial Cost (L2) |       Simplified Cost (L2) |
+|------|----------------------|----------------------------|
+| mean |             20.6585  |                   10.5799  |
+| std  |              7.82485 |                    3.27368 |
+| min  |              6.97623 |                    4.05423 |
+| 25%  |             14.969   |                    8.31298 |
+| 50%  |             19.1902  |                   10.1528  |
+| 75%  |             24.7936  |                   12.4421  |
+| 95%  |             35.3264  |                   16.5518  |
+| max  |             53.2503  |                   26.6381  |
+|      |     Filter Time (ms) |       CAPT Build Time (ms) |   Total Time (ms) |
+|------|----------------------|----------------------------|-------------------|
+| mean |              3.376   |                    4.37196 |          19.8227  |
+| std  |              1.32559 |                    1.70298 |          31.678   |
+| min  |              1.476   |                    1.44    |           3.98    |
+| 25%  |              2.6275  |                    3.129   |           7.31875 |
+| 50%  |              2.776   |                    3.7875  |          11.664   |
+| 75%  |              4.52225 |                    6.2365  |          19.9468  |
+| 95%  |              6.1671  |                    7.4751  |          58.0952  |
+| max  |              6.691   |                    8.955   |         471.304   |
+Solved / Valid / Total # Problems: 672 / 679 / 700
+Completed all problems in 8113.554 milliseconds
+Total time including Python overhead: 50923.907 milliseconds
+```
+
 ### [Orange Pi 5B](http://www.orangepi.org/html/hardWare/computerAndMicrocontrollers/details/Orange-Pi-5B.html)
 
 #### UR5

diff --git a/resources/ur5/ur5.srdf b/resources/ur5/ur5.srdf
@@ -139,7 +139,6 @@
     <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
     <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
     <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="wrist_2_link" link2="fts_robotside" reason="Adjacent" />
     <disable_collisions link1="fts_toolside" link2="ee_link" reason="Adjacent" />
     <disable_collisions link1="fts_robotside" link2="ee_link" reason="Adjacent" />
     <disable_collisions link1="fts_toolside" link2="robotiq_85_base_link" reason="Adjacent" />

diff --git a/scripts/README.md b/scripts/README.md
@@ -39,6 +39,10 @@ These scripts support standard planner and simplifier configuration arguments (s
 In addition, they both support the following arguments:
 - `problem`: which takes in either a single problem name (e.g., `table_pick`) or a list (e.g., `table_pick,table_under_pick`) to evaluate against a specific set of problems.
 - `dataset`: which describes the specific dataset of problems that should be loaded and inspected. See [Datasets](../resources/README.md#supported-planners) for more information.
+- `pointcloud`: instead of primitives, construct pointclouds from these primitive representations and plan against this representation using the CAPT datastructure.
+- `samples_per_object`: number of samples per each object for pointcloud generation.
+- `filter_radius`: pointcloud filtering radius. Will remove all redundant points that are within the specified radius.
+- `filter_cull`: remove points from the pointcloud that are farther than are reachable by the robot.
 
 `evaluate_mbm.py` also supports:
 - `trials`: the number of times to repeat each test.