feat: polytope collision checking via iterative projection

.gitignore

@@ -1,4 +1,5 @@
 build/
+build_test/
 .cache/
 compile_commands.json
 *.out.*

CMakeLists.txt

@@ -69,6 +69,7 @@ target_link_libraries_system(vamp_cpp
     Eigen3::Eigen
     nigh
     pdqsort
+    cdd
 )
 
 # Link SIMDxorshift if available

README.md

@@ -100,6 +100,7 @@ VAMP fetches the following external dependencies via [CPM](https://github.com/cp
 - [`nigh`](https://github.com/KavrakiLab/nigh): a fork of the original [`nigh`](https://github.com/UNC-Robotics/nigh) [[4]](#4) to better use our vector types
 - [`pdqsort`](https://github.com/orlp/pdqsort): for fast sorting
 - [`SIMDxorshift`](https://github.com/lemire/SIMDxorshift): alternative fast random numbers for x86 machines
+- [`cddlib`](https://github.com/cddlib/cddlib): for convex polytope vertex/halfspace conversion
 
 Download the code:
 ```bash
@@ -114,7 +115,7 @@ pip install .
 ```
 If you want to install all Python dependencies to run the examples, specify those optional dependencies:
 ```bash
-pip install .[examples,heightmaps]
+pip install .[examples,heightmaps,polytopes]
 ```
 If you have installed the `examples` dependencies, test your installation by running:
 ```bash
@@ -295,6 +296,12 @@ These objects can be created with the following:
 - `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.
+- `vamp.ConvexPolytope`: a convex polytope obstacle defined by halfspace planes and/or vertices. Can be constructed in several ways:
+  - `ConvexPolytope.from_vertices(vertices)`: from a numpy array of vertices
+  - `ConvexPolytope.from_planes(planes)`: from halfspace planes as `List[[nx, ny, nz, d]]` where the interior satisfies `nx*x + ny*y + nz*z <= d`
+  - `ConvexPolytope.from_both(vertices, planes)`: from both representations simultaneously
+  - `vamp.mesh_to_polytopes(filename, position, scale, convex_decomposition, name)`: helper function to load a mesh file and convert it to a list of `ConvexPolytope` obstacles. Supports convex hull or convex decomposition of non-convex meshes. Requires `trimesh`.
+
 See the `src/impl/vamp/collision/` folder for more information.
 
 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)`.
@@ -346,7 +353,7 @@ Inside `impl/vamp`, the code is divided into the following directories:
 - [ ] Batch configuration validation
 - [ ] Planning subgroups
 - [X] Object attachment at end-effector
-- [ ] Mesh collision checking
+- [X] Mesh collision checking
 - [X] Pointcloud collision checking
 - [ ] Manifold-constrained planning
 - [ ] Time-optimal trajectory parameterization

cmake/Dependencies.cmake

@@ -30,6 +30,42 @@ if(VAMP_INSTALL_CPP_LIBRARY)
   export(EXPORT pdqsort_TARGETS FILE ${CMAKE_CURRENT_BINARY_DIR}/cmake/pdqsortTargets.cmake NAMESPACE pdqsort::)
 endif()
 
+# cddlib for convex polytope vertex/halfspace conversion
+CPMAddPackage(
+  NAME cddlib
+  GITHUB_REPOSITORY cddlib/cddlib
+  GIT_TAG 0.94m
+)
+
+if(cddlib_SOURCE_DIR)
+  # cddlib uses autotools...
+  add_library(cdd STATIC
+    ${cddlib_SOURCE_DIR}/lib-src/cddcore.c
+    ${cddlib_SOURCE_DIR}/lib-src/cddlp.c
+    ${cddlib_SOURCE_DIR}/lib-src/cddmp.c
+    ${cddlib_SOURCE_DIR}/lib-src/cddio.c
+    ${cddlib_SOURCE_DIR}/lib-src/cddlib.c
+    ${cddlib_SOURCE_DIR}/lib-src/cddproj.c
+    ${cddlib_SOURCE_DIR}/lib-src/setoper.c
+  )
+  target_include_directories(cdd PUBLIC
+    $<BUILD_INTERFACE:${cddlib_SOURCE_DIR}/lib-src>
+  )
+  set_target_properties(cdd PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+  if(VAMP_INSTALL_CPP_LIBRARY)
+    install(TARGETS cdd
+          EXPORT cdd_TARGETS
+          LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+          ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+          RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+          INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+    )
+    install(EXPORT cdd_TARGETS FILE cddTargets.cmake NAMESPACE cdd:: DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/cdd)
+    export(EXPORT cdd_TARGETS FILE ${CMAKE_CURRENT_BINARY_DIR}/cmake/cddTargets.cmake NAMESPACE cdd::)
+  endif()
+endif()
+
 # SIMDxorshift for x86_64 systems (includes macOS Intel and Linux x86_64)
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64" OR CMAKE_SYSTEM_PROCESSOR STREQUAL "AMD64")
   CPMAddPackage("gh:lemire/SIMDxorshift#857c1a01df53cf1ee1ae8db3238f0ef42ef8e490")

pyproject.toml

@@ -20,6 +20,7 @@ dependencies = ["numpy"]
 [project.optional-dependencies]
 examples = ["fire", "pybullet", "pyyaml", "tqdm", "pandas", "tabulate", "xmltodict"]
 heightmaps = ["pillow"]
+polytopes = ["trimesh", "vhacdx", "scipy"]
 
 [project.urls]
 Homepage = "https://github.com/KavrakiLab/vamp"

scripts/mesh_test.py

@@ -0,0 +1,104 @@
+from pathlib import Path
+import vamp
+from fire import Fire
+
+# Starting configuration
+a = [-1., 1., 0., -0.8, 0, 2.5, 0.785]
+
+# Goal configuration
+b = [1., 1., 0., -0.8, 0, 2.5, 0.785]
+
+
+def main(
+    mesh_file: str = None,
+    position: tuple = (0.5, 0.0, 0.0),
+    scale: float = 0.01,
+    decompose: bool = False,
+    max_convex_hulls: int = 10,
+    benchmark: bool = False,
+    n_trials: int = 100,
+    visualize: bool = True,
+    planner: str = "rrtc",
+    sampler_name: str = "halton",
+    skip_rng_iterations: int = 0,
+    **kwargs,
+    ):
+
+    if mesh_file is None:
+        mesh_path = Path(__file__).parent.parent / "resources" / "objects" / "Stanford_Bunny.stl"
+    else:
+        mesh_path = Path(mesh_file)
+
+    if not mesh_path.exists():
+        raise FileNotFoundError(f"Mesh file not found: {mesh_path}")
+
+    polytopes = vamp.mesh_to_polytopes(
+        mesh_path,
+        position = position,
+        scale = scale,
+        convex_decomposition = decompose,
+        name = "mesh",
+        maxConvexHulls = max_convex_hulls
+        )
+
+    (vamp_module, planner_func, plan_settings,
+     simp_settings) = vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs)
+
+    sampler = getattr(vamp_module, sampler_name)()
+    sampler.skip(skip_rng_iterations)
+
+    def create_environment():
+        e = vamp.Environment()
+        for polytope in polytopes:
+            e.add_polytope(polytope)
+        return e
+
+    if benchmark:
+        import pandas as pd
+
+        results = []
+        for _ in range(n_trials):
+            e = create_environment()
+
+            if vamp.panda.validate(a, e) and vamp.panda.validate(b, e):
+                result = planner_func(a, b, e, plan_settings, sampler)
+                simple = vamp_module.simplify(result.path, e, simp_settings, sampler)
+                results.append(vamp.results_to_dict(result, simple))
+
+        df = pd.DataFrame.from_dict(results)
+
+        # Convert to microseconds
+        df["planning_time"] = df["planning_time"].dt.microseconds
+        df["simplification_time"] = df["simplification_time"].dt.microseconds
+
+        # Get summary statistics
+        stats = df[[
+            "planning_time",
+            "simplification_time",
+            "initial_path_cost",
+            "simplified_path_cost",
+            "planning_iterations"
+            ]].describe()
+
+        print(stats)
+
+    if visualize:
+        from vamp import pybullet_interface as vpb
+
+        robot_dir = Path(__file__).parent.parent / 'resources' / 'panda'
+        sim = vpb.PyBulletSimulator(str(robot_dir / f"panda_spherized.urdf"), vamp_module.joint_names(), True)
+
+        e = create_environment()
+        for polytope in polytopes:
+            sim.add_polytope(polytope)
+
+        result = planner_func(a, b, e, plan_settings, sampler)
+        simple = vamp_module.simplify(result.path, e, simp_settings, sampler)
+
+        simple.path.interpolate_to_resolution(vamp.panda.resolution())
+
+        sim.animate(simple.path)
+
+
+if __name__ == "__main__":
+    Fire(main)

src/impl/vamp/bindings/environment.cc

@@ -106,6 +106,101 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
         .def_ro("zs", &vc::HeightField<float>::zs)
         .def_ro("data", &vc::HeightField<float>::data);
 
+    nb::class_<vc::ConvexPolytope<float>>(pymodule, "ConvexPolytope")
+        .def(
+            "__init__",
+            [](vc::ConvexPolytope<float> *q, const std::vector<std::array<float, 4>> &planes)
+            { new (q) vc::ConvexPolytope<float>(vf::polytope::from_planes(planes)); },
+            "planes"_a,
+            "Constructor from halfplanes as [nx, ny, nz, d].")
+        .def(
+            "__init__",
+            [](vc::ConvexPolytope<float> *q,
+               const nb::ndarray<float, nb::shape<-1, 3>, nb::device::cpu> &vertices)
+            {
+                std::vector<float> vx, vy, vz;
+                const std::size_t num_vertices = vertices.shape(0);
+                vx.reserve(num_vertices);
+                vy.reserve(num_vertices);
+                vz.reserve(num_vertices);
+
+                for (std::size_t i = 0; i < num_vertices; ++i)
+                {
+                    vx.emplace_back(vertices(i, 0));
+                    vy.emplace_back(vertices(i, 1));
+                    vz.emplace_back(vertices(i, 2));
+                }
+
+                new (q) vc::ConvexPolytope<float>(vf::polytope::from_vertices(vx, vy, vz));
+            },
+            "vertices"_a,
+            "Constructor from vertices array.")
+        .def_static(
+            "from_planes",
+            &vf::polytope::from_planes,
+            "planes"_a,
+            "Create from halfplanes as List[[nx, ny, nz, d]].")
+        .def_static(
+            "from_vertices",
+            [](const nb::ndarray<float, nb::shape<-1, 3>, nb::device::cpu> &vertices)
+            {
+                std::vector<float> vx, vy, vz;
+                const std::size_t num_vertices = vertices.shape(0);
+                vx.reserve(num_vertices);
+                vy.reserve(num_vertices);
+                vz.reserve(num_vertices);
+
+                for (std::size_t i = 0; i < num_vertices; ++i)
+                {
+                    vx.emplace_back(vertices(i, 0));
+                    vy.emplace_back(vertices(i, 1));
+                    vz.emplace_back(vertices(i, 2));
+                }
+
+                return vf::polytope::from_vertices(vx, vy, vz);
+            },
+            "vertices"_a,
+            "Create from vertices array.")
+        .def_static(
+            "from_both",
+            [](const nb::ndarray<float, nb::shape<-1, 3>, nb::device::cpu> &vertices,
+               const nb::ndarray<float, nb::shape<-1, 4>, nb::device::cpu> &planes)
+            {
+                std::vector<std::array<float, 3>> verts;
+                std::vector<std::array<float, 4>> plns;
+                const std::size_t num_vertices = vertices.shape(0);
+                const std::size_t num_planes = planes.shape(0);
+                verts.reserve(num_vertices);
+                plns.reserve(num_planes);
+
+                for (auto i = 0U; i < num_vertices; ++i)
+                {
+                    verts.emplace_back(std::array<float, 3>{vertices(i, 0), vertices(i, 1), vertices(i, 2)});
+                }
+
+                for (auto i = 0U; i < num_planes; ++i)
+                {
+                    plns.emplace_back(
+                        std::array<float, 4>{planes(i, 0), planes(i, 1), planes(i, 2), planes(i, 3)});
+                }
+
+                return vf::polytope::from_both(verts, plns);
+            },
+            "vertices"_a,
+            "planes"_a,
+            "Create from both vertices array and planes array.")
+        .def_ro("num_planes", &vc::ConvexPolytope<float>::num_planes)
+        .def_ro("nx", &vc::ConvexPolytope<float>::nx)
+        .def_ro("ny", &vc::ConvexPolytope<float>::ny)
+        .def_ro("nz", &vc::ConvexPolytope<float>::nz)
+        .def_ro("d", &vc::ConvexPolytope<float>::d)
+        .def_ro("num_vertices", &vc::ConvexPolytope<float>::num_vertices)
+        .def_ro("vx", &vc::ConvexPolytope<float>::vx)
+        .def_ro("vy", &vc::ConvexPolytope<float>::vy)
+        .def_ro("vz", &vc::ConvexPolytope<float>::vz)
+        .def_ro("min_distance", &vc::ConvexPolytope<float>::min_distance)
+        .def_rw("name", &vc::ConvexPolytope<float>::name);
+
     nb::class_<vc::Environment<float>>(pymodule, "Environment")
         .def(nb::init<>())
         .def(
@@ -147,6 +242,13 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule)
             "add_heightfield",
             [](vc::Environment<float> &e, const vc::HeightField<float> &s)
             { e.heightfields.emplace_back(s); })
+        .def(
+            "add_polytope",
+            [](vc::Environment<float> &e, const vc::ConvexPolytope<float> &p)
+            {
+                e.polytopes.emplace_back(p);
+                e.sort();
+            })
         .def(
             "add_pointcloud",
             [](vc::Environment<float> &e,

src/impl/vamp/collision/environment.hh

@@ -21,6 +21,7 @@ namespace vamp::collision
         std::vector<Cuboid<DataT>> cuboids;
         std::vector<Cuboid<DataT>> z_aligned_cuboids;
         std::vector<HeightField<DataT>> heightfields;
+        std::vector<ConvexPolytope<DataT>> polytopes;
         std::vector<CAPT> pointclouds;
         std::optional<Attachment<DataT>> attachments;
 
@@ -35,6 +36,7 @@ namespace vamp::collision
           , cuboids(other.cuboids.begin(), other.cuboids.end())
           , z_aligned_cuboids(other.z_aligned_cuboids.begin(), other.z_aligned_cuboids.end())
           , heightfields(other.heightfields.begin(), other.heightfields.end())
+          , polytopes(other.polytopes.begin(), other.polytopes.end())
           , pointclouds(other.pointclouds.begin(), other.pointclouds.end())
           , attachments(other.template clone_attachments<DataT>())
         {
@@ -66,6 +68,10 @@ namespace vamp::collision
                 z_aligned_cuboids.begin(),
                 z_aligned_cuboids.end(),
                 [](const auto &a, const auto &b) { return a.min_distance < b.min_distance; });
+            std::sort(
+                polytopes.begin(),
+                polytopes.end(),
+                [](const auto &a, const auto &b) { return a.min_distance < b.min_distance; });
         }
 
     private: