diff --git a/.gitignore b/.gitignore index 5760c9e5..01f5572f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ build/ +build_test/ .cache/ compile_commands.json *.out.* diff --git a/CMakeLists.txt b/CMakeLists.txt index 1217634c..cc4162be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ target_link_libraries_system(vamp_cpp Eigen3::Eigen nigh pdqsort + cdd ) # Link SIMDxorshift if available diff --git a/README.md b/README.md index c9461808..a46db6b8 100644 --- a/README.md +++ b/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 diff --git a/cmake/Dependencies.cmake b/cmake/Dependencies.cmake index 9aa5effe..869562b2 100644 --- a/cmake/Dependencies.cmake +++ b/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 + $ + ) + 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") diff --git a/pyproject.toml b/pyproject.toml index f24e6cde..e19fad16 100644 --- a/pyproject.toml +++ b/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" diff --git a/resources/objects/Stanford_Bunny.stl b/resources/objects/Stanford_Bunny.stl new file mode 100644 index 00000000..bec02fcb Binary files /dev/null and b/resources/objects/Stanford_Bunny.stl differ diff --git a/scripts/mesh_test.py b/scripts/mesh_test.py new file mode 100644 index 00000000..0815da48 --- /dev/null +++ b/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) diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc index ae296b84..b254f0b6 100644 --- a/src/impl/vamp/bindings/environment.cc +++ b/src/impl/vamp/bindings/environment.cc @@ -106,6 +106,101 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule) .def_ro("zs", &vc::HeightField::zs) .def_ro("data", &vc::HeightField::data); + nb::class_>(pymodule, "ConvexPolytope") + .def( + "__init__", + [](vc::ConvexPolytope *q, const std::vector> &planes) + { new (q) vc::ConvexPolytope(vf::polytope::from_planes(planes)); }, + "planes"_a, + "Constructor from halfplanes as [nx, ny, nz, d].") + .def( + "__init__", + [](vc::ConvexPolytope *q, + const nb::ndarray, nb::device::cpu> &vertices) + { + std::vector 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(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, nb::device::cpu> &vertices) + { + std::vector 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, nb::device::cpu> &vertices, + const nb::ndarray, nb::device::cpu> &planes) + { + std::vector> verts; + std::vector> 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{vertices(i, 0), vertices(i, 1), vertices(i, 2)}); + } + + for (auto i = 0U; i < num_planes; ++i) + { + plns.emplace_back( + std::array{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::num_planes) + .def_ro("nx", &vc::ConvexPolytope::nx) + .def_ro("ny", &vc::ConvexPolytope::ny) + .def_ro("nz", &vc::ConvexPolytope::nz) + .def_ro("d", &vc::ConvexPolytope::d) + .def_ro("num_vertices", &vc::ConvexPolytope::num_vertices) + .def_ro("vx", &vc::ConvexPolytope::vx) + .def_ro("vy", &vc::ConvexPolytope::vy) + .def_ro("vz", &vc::ConvexPolytope::vz) + .def_ro("min_distance", &vc::ConvexPolytope::min_distance) + .def_rw("name", &vc::ConvexPolytope::name); + nb::class_>(pymodule, "Environment") .def(nb::init<>()) .def( @@ -147,6 +242,13 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule) "add_heightfield", [](vc::Environment &e, const vc::HeightField &s) { e.heightfields.emplace_back(s); }) + .def( + "add_polytope", + [](vc::Environment &e, const vc::ConvexPolytope &p) + { + e.polytopes.emplace_back(p); + e.sort(); + }) .def( "add_pointcloud", [](vc::Environment &e, diff --git a/src/impl/vamp/collision/environment.hh b/src/impl/vamp/collision/environment.hh index a380bf85..047055e5 100644 --- a/src/impl/vamp/collision/environment.hh +++ b/src/impl/vamp/collision/environment.hh @@ -21,6 +21,7 @@ namespace vamp::collision std::vector> cuboids; std::vector> z_aligned_cuboids; std::vector> heightfields; + std::vector> polytopes; std::vector pointclouds; std::optional> 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()) { @@ -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: diff --git a/src/impl/vamp/collision/factory.hh b/src/impl/vamp/collision/factory.hh index 6c201a14..5c6a9eb8 100644 --- a/src/impl/vamp/collision/factory.hh +++ b/src/impl/vamp/collision/factory.hh @@ -6,6 +6,7 @@ #include #include #include +#include #include @@ -421,4 +422,85 @@ namespace vamp::collision::factory data); } } // namespace heightfield + + namespace polytope + { + // Construct a convex polytope from arrays of plane coefficients + // Each plane is defined as nx*x + ny*y + nz*z <= d + // Normals should be unit vectors pointing outward from the polytope interior + inline static auto flat( + const std::vector &nx, + const std::vector &ny, + const std::vector &nz, + const std::vector &d) -> collision::ConvexPolytope + { + auto data = collision::halfspaces_to_vertices(nx, ny, nz, d); + return collision::ConvexPolytope( + data.nx.size(), data.nx, data.ny, data.nz, data.d, data.vx.size(), data.vx, data.vy, data.vz); + } + + // Construct from a vector of halfspaces + inline static auto from_planes(const std::vector> &planes) + -> collision::ConvexPolytope + { + auto data = collision::halfspaces_to_vertices(planes); + return collision::ConvexPolytope( + data.nx.size(), data.nx, data.ny, data.nz, data.d, data.vx.size(), data.vx, data.vy, data.vz); + } + + // Construct from vertices + inline static auto from_vertices( + const std::vector &vx, + const std::vector &vy, + const std::vector &vz) -> collision::ConvexPolytope + { + auto data = collision::vertices_to_halfspaces(vx, vy, vz); + return collision::ConvexPolytope( + data.nx.size(), data.nx, data.ny, data.nz, data.d, data.vx.size(), data.vx, data.vy, data.vz); + } + + // Construct from a vector of vertices + inline static auto from_vertices(const std::vector> &vertices) + -> collision::ConvexPolytope + { + auto data = collision::vertices_to_halfspaces(vertices); + return collision::ConvexPolytope( + data.nx.size(), data.nx, data.ny, data.nz, data.d, data.vx.size(), data.vx, data.vy, data.vz); + } + + // Construct from both representations + inline static auto from_both( + const std::vector> &vertices, + const std::vector> &planes) noexcept -> collision::ConvexPolytope + { + std::vector nx, ny, nz, d; + nx.reserve(planes.size()); + ny.reserve(planes.size()); + nz.reserve(planes.size()); + d.reserve(planes.size()); + + for (const auto &plane : planes) + { + nx.emplace_back(plane[0]); + ny.emplace_back(plane[1]); + nz.emplace_back(plane[2]); + d.emplace_back(plane[3]); + } + + std::vector vx, vy, vz; + vx.reserve(vertices.size()); + vy.reserve(vertices.size()); + vz.reserve(vertices.size()); + + for (const auto &v : vertices) + { + vx.emplace_back(v[0]); + vy.emplace_back(v[1]); + vz.emplace_back(v[2]); + } + + return collision::ConvexPolytope( + planes.size(), nx, ny, nz, d, vertices.size(), vx, vy, vz); + } + } // namespace polytope } // namespace vamp::collision::factory diff --git a/src/impl/vamp/collision/polytope_convert.hh b/src/impl/vamp/collision/polytope_convert.hh new file mode 100644 index 00000000..c1b8b327 --- /dev/null +++ b/src/impl/vamp/collision/polytope_convert.hh @@ -0,0 +1,216 @@ +#pragma once + +#include +#include +#include +#include + +extern "C" +{ +#include +#include +} + +namespace vamp::collision +{ + struct PolytopeData + { + std::vector nx, ny, nz, d; + std::vector vx, vy, vz; + }; + + inline void cdd_init() + { + static bool initialized = false; + if (not initialized) + { + dd_set_global_constants(); + initialized = true; + } + } + + inline auto vertices_to_halfspaces( + const std::vector &vx, + const std::vector &vy, + const std::vector &vz) -> PolytopeData + { + cdd_init(); + + const std::size_t num_vertices = vx.size(); + if (num_vertices < 4) + { + throw std::invalid_argument("Need at least 4 vertices for a 3D convex polytope"); + } + + PolytopeData result; + result.vx = vx; + result.vy = vy; + result.vz = vz; + + // Each row is [1, x, y, z] for a vertex (1 indicates vertex, not ray) + dd_MatrixPtr vertices_matrix = dd_CreateMatrix(num_vertices, 4); + vertices_matrix->representation = dd_Generator; + vertices_matrix->numbtype = dd_Real; + + for (auto i = 0U; i < num_vertices; ++i) + { + dd_set_d(vertices_matrix->matrix[i][0], 1.0); + dd_set_d(vertices_matrix->matrix[i][1], static_cast(vx[i])); + dd_set_d(vertices_matrix->matrix[i][2], static_cast(vy[i])); + dd_set_d(vertices_matrix->matrix[i][3], static_cast(vz[i])); + } + + dd_ErrorType err = dd_NoError; + dd_PolyhedraPtr poly = dd_DDMatrix2Poly(vertices_matrix, &err); + + if (err != dd_NoError or poly == nullptr) + { + dd_FreeMatrix(vertices_matrix); + if (poly) + { + dd_FreePolyhedra(poly); + } + throw std::runtime_error("cddlib: Failed to convert vertices to halfspaces"); + } + + dd_MatrixPtr inequalities = dd_CopyInequalities(poly); + + result.nx.reserve(inequalities->rowsize); + result.ny.reserve(inequalities->rowsize); + result.nz.reserve(inequalities->rowsize); + result.d.reserve(inequalities->rowsize); + + for (dd_rowrange i = 0; i < inequalities->rowsize; ++i) + { + // cddlib format: b + a1*x1 + a2*x2 + a3*x3 >= 0 + // We want: nx*x + ny*y + nz*z <= d + // So: -a1*x - a2*y - a3*z <= b + double b = dd_get_d(inequalities->matrix[i][0]); + double a1 = dd_get_d(inequalities->matrix[i][1]); + double a2 = dd_get_d(inequalities->matrix[i][2]); + double a3 = dd_get_d(inequalities->matrix[i][3]); + + // Normalize the plane equation + double norm = std::sqrt(a1 * a1 + a2 * a2 + a3 * a3); + if (norm > 1e-10) + { + result.nx.emplace_back(static_cast(-a1 / norm)); + result.ny.emplace_back(static_cast(-a2 / norm)); + result.nz.emplace_back(static_cast(-a3 / norm)); + result.d.emplace_back(static_cast(b / norm)); + } + } + + dd_FreeMatrix(inequalities); + dd_FreePolyhedra(poly); + dd_FreeMatrix(vertices_matrix); + + return result; + } + + inline auto halfspaces_to_vertices( + const std::vector &nx, + const std::vector &ny, + const std::vector &nz, + const std::vector &d) -> PolytopeData + { + cdd_init(); + + const std::size_t num_planes = nx.size(); + if (num_planes < 4) + { + throw std::invalid_argument("Need at least 4 halfspaces for a bounded 3D polytope"); + } + + PolytopeData result; + result.nx = nx; + result.ny = ny; + result.nz = nz; + result.d = d; + + dd_MatrixPtr ineq_matrix = dd_CreateMatrix(num_planes, 4); + ineq_matrix->representation = dd_Inequality; + ineq_matrix->numbtype = dd_Real; + + for (std::size_t i = 0; i < num_planes; ++i) + { + dd_set_d(ineq_matrix->matrix[i][0], static_cast(d[i])); + dd_set_d(ineq_matrix->matrix[i][1], static_cast(-nx[i])); + dd_set_d(ineq_matrix->matrix[i][2], static_cast(-ny[i])); + dd_set_d(ineq_matrix->matrix[i][3], static_cast(-nz[i])); + } + + dd_ErrorType err = dd_NoError; + dd_PolyhedraPtr poly = dd_DDMatrix2Poly(ineq_matrix, &err); + + if (err != dd_NoError || poly == nullptr) + { + dd_FreeMatrix(ineq_matrix); + if (poly) + { + dd_FreePolyhedra(poly); + } + throw std::runtime_error("cddlib: Failed to convert halfspaces to vertices"); + } + + dd_MatrixPtr generators = dd_CopyGenerators(poly); + + result.vx.reserve(generators->rowsize); + result.vy.reserve(generators->rowsize); + result.vz.reserve(generators->rowsize); + + for (dd_rowrange i = 0; i < generators->rowsize; ++i) + { + double type = dd_get_d(generators->matrix[i][0]); + if (type > 0.5) + { + result.vx.emplace_back(static_cast(dd_get_d(generators->matrix[i][1]))); + result.vy.emplace_back(static_cast(dd_get_d(generators->matrix[i][2]))); + result.vz.emplace_back(static_cast(dd_get_d(generators->matrix[i][3]))); + } + } + + dd_FreeMatrix(generators); + dd_FreePolyhedra(poly); + dd_FreeMatrix(ineq_matrix); + + return result; + } + + inline auto vertices_to_halfspaces(const std::vector> &vertices) -> PolytopeData + { + std::vector vx, vy, vz; + vx.reserve(vertices.size()); + vy.reserve(vertices.size()); + vz.reserve(vertices.size()); + + for (const auto &v : vertices) + { + vx.emplace_back(v[0]); + vy.emplace_back(v[1]); + vz.emplace_back(v[2]); + } + + return vertices_to_halfspaces(vx, vy, vz); + } + + inline auto halfspaces_to_vertices(const std::vector> &planes) -> PolytopeData + { + std::vector nx, ny, nz, d; + nx.reserve(planes.size()); + ny.reserve(planes.size()); + nz.reserve(planes.size()); + d.reserve(planes.size()); + + for (const auto &p : planes) + { + nx.emplace_back(p[0]); + ny.emplace_back(p[1]); + nz.emplace_back(p[2]); + d.emplace_back(p[3]); + } + + return halfspaces_to_vertices(nx, ny, nz, d); + } + +} // namespace vamp::collision diff --git a/src/impl/vamp/collision/shapes.hh b/src/impl/vamp/collision/shapes.hh index 19b001ba..98317693 100644 --- a/src/impl/vamp/collision/shapes.hh +++ b/src/impl/vamp/collision/shapes.hh @@ -2,10 +2,14 @@ #include #include +#include +#include +#include #include #include +#include #include namespace vamp::collision @@ -310,4 +314,181 @@ namespace vamp::collision { } }; + + // A convex polytope with dual representation: + // - H-representation: half-planes + // - V-representation: vertices + template + struct ConvexPolytope : public Shape + { + // Halfspaces + std::size_t num_planes; + std::vector nx; // plane normals + std::vector ny; + std::vector nz; + std::vector d; // plane offsets (n.x <= d defines interior) + + // Vertices + std::size_t num_vertices; + std::vector vx; + std::vector vy; + std::vector vz; + + // Oriented bounding box for broadphase + Cuboid obb; + + ConvexPolytope() = default; + + explicit ConvexPolytope( + std::size_t num_planes, + const std::vector &nx, + const std::vector &ny, + const std::vector &nz, + const std::vector &d, + std::size_t num_vertices, + const std::vector &vx, + const std::vector &vy, + const std::vector &vz) + : Shape() + , num_planes(num_planes) + , nx(nx) + , ny(ny) + , nz(nz) + , d(d) + , num_vertices(num_vertices) + , vx(vx) + , vy(vy) + , vz(vz) + { + obb = compute_obb(); + Shape::min_distance = compute_min_distance(); + } + + inline auto compute_min_distance() -> DataT + { + // Minimum distance from origin to any vertex + float min_dist = std::numeric_limits::max(); + for (auto i = 0U; i < num_vertices; ++i) + { + float dist = std::sqrt(vx[i] * vx[i] + vy[i] * vy[i] + vz[i] * vz[i]); + min_dist = std::min(min_dist, dist); + } + + return DataT(min_dist); + } + + inline auto compute_obb() -> Cuboid + { + float cx = 0, cy = 0, cz = 0; + for (auto i = 0U; i < num_vertices; ++i) + { + cx += vx[i]; + cy += vy[i]; + cz += vz[i]; + } + cx /= num_vertices; + cy /= num_vertices; + cz /= num_vertices; + + Eigen::Matrix3f cov = Eigen::Matrix3f::Zero(); + for (auto i = 0U; i < num_vertices; ++i) + { + const float dx = vx[i] - cx; + const float dy = vy[i] - cy; + const float dz = vz[i] - cz; + cov(0, 0) += dx * dx; + cov(0, 1) += dx * dy; + cov(0, 2) += dx * dz; + cov(1, 1) += dy * dy; + cov(1, 2) += dy * dz; + cov(2, 2) += dz * dz; + } + cov(1, 0) = cov(0, 1); + cov(2, 0) = cov(0, 2); + cov(2, 1) = cov(1, 2); + + Eigen::SelfAdjointEigenSolver solver(cov); + Eigen::Matrix3f axes = solver.eigenvectors(); + + if (axes.determinant() < 0) + { + axes.col(0) *= -1; + } + + float min_0 = std::numeric_limits::max(); + float max_0 = std::numeric_limits::lowest(); + float min_1 = std::numeric_limits::max(); + float max_1 = std::numeric_limits::lowest(); + float min_2 = std::numeric_limits::max(); + float max_2 = std::numeric_limits::lowest(); + + const Eigen::Vector3f axis_0 = axes.col(0); + const Eigen::Vector3f axis_1 = axes.col(1); + const Eigen::Vector3f axis_2 = axes.col(2); + + for (auto i = 0U; i < num_vertices; ++i) + { + const Eigen::Vector3f v(vx[i], vy[i], vz[i]); + const float proj_0 = v.dot(axis_0); + const float proj_1 = v.dot(axis_1); + const float proj_2 = v.dot(axis_2); + + min_0 = std::min(min_0, proj_0); + max_0 = std::max(max_0, proj_0); + min_1 = std::min(min_1, proj_1); + max_1 = std::max(max_1, proj_1); + min_2 = std::min(min_2, proj_2); + max_2 = std::max(max_2, proj_2); + } + + const float mid_0 = (min_0 + max_0) * 0.5f; + const float mid_1 = (min_1 + max_1) * 0.5f; + const float mid_2 = (min_2 + max_2) * 0.5f; + Eigen::Vector3f center = mid_0 * axis_0 + mid_1 * axis_1 + mid_2 * axis_2; + + const float half_0 = (max_0 - min_0) * 0.5f; + const float half_1 = (max_1 - min_1) * 0.5f; + const float half_2 = (max_2 - min_2) * 0.5f; + + return Cuboid( + DataT(center.x()), + DataT(center.y()), + DataT(center.z()), + DataT(axis_0.x()), + DataT(axis_0.y()), + DataT(axis_0.z()), + DataT(axis_1.x()), + DataT(axis_1.y()), + DataT(axis_1.z()), + DataT(axis_2.x()), + DataT(axis_2.y()), + DataT(axis_2.z()), + DataT(half_0), + DataT(half_1), + DataT(half_2)); + } + + template + explicit ConvexPolytope(const ConvexPolytope &other) + : Shape(other) + , num_planes(other.num_planes) + , nx(other.nx.size()) + , ny(other.ny.size()) + , nz(other.nz.size()) + , d(other.d.size()) + , num_vertices(other.num_vertices) + , vx(other.vx) + , vy(other.vy) + , vz(other.vz) + , obb(other.obb) + { + for (auto i = 0U; i < other.nx.size(); ++i) + { + nx[i] = DataT(other.nx[i]); + ny[i] = DataT(other.ny[i]); + nz[i] = DataT(other.nz[i]); + d[i] = DataT(other.d[i]); + } + } + }; } // namespace vamp::collision diff --git a/src/impl/vamp/collision/sphere_polytope.hh b/src/impl/vamp/collision/sphere_polytope.hh new file mode 100644 index 00000000..ad67a3f4 --- /dev/null +++ b/src/impl/vamp/collision/sphere_polytope.hh @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include + +#include + +namespace vamp::collision +{ + template + inline auto sphere_polytope( + const ConvexPolytope &p, + const DataT &cx, + const DataT &cy, + const DataT &cz, + const DataT &r) noexcept -> DataT + { + const auto rsq = r * r; + auto obb_dist = sphere_cuboid(p.obb, cx, cy, cz, rsq); + + if (obb_dist.test_zero()) + { + return obb_dist; + } + + DataT max_dist = DataT::fill(-std::numeric_limits::max()); + + // TODO: Proper GJK? Seems OK for now. + for (auto i = 0U; i < p.num_planes; ++i) + { + // dist = n.C - d - r (positive = separated) + auto dist = dot_3(p.nx[i], p.ny[i], p.nz[i], cx, cy, cz) - p.d[i] - r; + max_dist = max_dist.max(dist); + + if (max_dist.test_zero()) + { + break; + } + } + + return max_dist; + } + + template + inline auto sphere_polytope(const ConvexPolytope &p, const Sphere &s) noexcept -> DataT + { + return sphere_polytope(p, s.x, s.y, s.z, s.r); + } +} // namespace vamp::collision diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index b555c13e..f460bc72 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -6,6 +6,7 @@ #include #include #include +#include #include namespace vamp @@ -137,6 +138,20 @@ namespace vamp } } + for (const auto &ep : e.polytopes) + { + const auto diff = ep.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + if (not collision::sphere_polytope(ep, sx, sy, sz, sr).test_zero()) + { + return true; + } + } + const std::array positions = {sx, sy, sz}; for (const auto &pc : e.pointclouds) { @@ -245,6 +260,20 @@ namespace vamp } } + for (const auto &ep : e.polytopes) + { + const auto diff = ep.min_distance - max_extent; + if (diff.test_zero()) + { + break; + } + + if (not collision::sphere_polytope(ep, sx, sy, sz, sr).test_zero()) + { + objects.emplace_back(ep.name); + } + } + return objects; } diff --git a/src/vamp/__init__.py b/src/vamp/__init__.py index 2bb88671..e38b2ddd 100644 --- a/src/vamp/__init__.py +++ b/src/vamp/__init__.py @@ -1,6 +1,7 @@ __all__ = [ "robots", "png_to_heightfield", + "mesh_to_polytopes", "configure_robot_and_planner_with_kwargs", "problem_dict_to_vamp", "results_to_dict", @@ -9,6 +10,7 @@ "Sphere", "Cuboid", "Cylinder", + "ConvexPolytope", "RRTCSettings", "PRMSettings", "PRMNeighborParams", @@ -21,8 +23,7 @@ ] from pathlib import Path -from typing import Any, Dict, Optional, Tuple, Union, List -import importlib +from typing import Any, Dict, Tuple, Union, List from numpy import float32 from numpy.typing import NDArray @@ -32,6 +33,7 @@ from ._core import Sphere as Sphere from ._core import Cuboid as Cuboid from ._core import Cylinder as Cylinder +from ._core import ConvexPolytope as ConvexPolytope from ._core import Attachment as Attachment from ._core import Environment as Environment from ._core import PRMNeighborParams as PRMNeighborParams @@ -66,6 +68,54 @@ def png_to_heightfield( return _core.make_heightfield(center, scaling, array.shape, list(array.flatten())) +def mesh_to_polytopes( + filename: Path, + position: Tuple[float, float, float] = (0.0, 0.0, 0.0), + scale: float = 1.0, + convex_decomposition: bool = False, + name: str = "mesh", + **decomposition_kwargs, + ) -> List[ConvexPolytope]: + import trimesh + import numpy as np + + mesh = trimesh.load(str(filename), force = 'mesh') + + def hull_to_polytope(hull, scale, position, polytope_name): + vertices = (np.array(hull.vertices) * scale + position).astype(np.float32) + normals = np.array(hull.face_normals).astype(np.float32) + face_vertices = hull.vertices[hull.faces[:, 0]] + d_orig = np.sum(normals * face_vertices, axis = 1) + d_transformed = (d_orig * scale + np.sum(normals * position, axis = 1)).astype(np.float32) + planes = np.column_stack([normals, d_transformed]).astype(np.float32) + + polytope = ConvexPolytope.from_both(vertices, planes) + polytope.name = polytope_name + return polytope + + if convex_decomposition: + parts = trimesh.decomposition.convex_decomposition(mesh, **decomposition_kwargs) + if not isinstance(parts, list): + parts = [parts] + + polytopes = [] + for i, part_data in enumerate(parts): + if isinstance(part_data, dict): + part_mesh = trimesh.Trimesh(**part_data) + else: + part_mesh = part_data + + hull = part_mesh.convex_hull + polytope_name = f"{name}_{i}" if len(parts) > 1 else name + polytopes.append(hull_to_polytope(hull, scale, position, polytope_name)) + + return polytopes + + else: + hull = mesh.convex_hull + return [hull_to_polytope(hull, scale, position, name)] + + def configure_robot_and_planner_with_kwargs(robot_name: str, planner_name: str, **kwargs): robot_module = getattr(_core, robot_name) try: diff --git a/src/vamp/pybullet_interface.py b/src/vamp/pybullet_interface.py index 2c5cdc05..e0ac502b 100644 --- a/src/vamp/pybullet_interface.py +++ b/src/vamp/pybullet_interface.py @@ -253,6 +253,62 @@ def add_sphere( return multibody_id + def add_polytope( + self, + polytope, + name: Optional[str] = None, + color: Optional[Union[List[float], str]] = None, + ): + from scipy.spatial import ConvexHull + + vertices = np.column_stack([polytope.vx, polytope.vy, polytope.vz]) + hull = ConvexHull(vertices) + + # Normals aren't always outward facing... + centroid = vertices.mean(axis = 0) + corrected_simplices = [] + for simplex in hull.simplices: + v0, v1, v2 = vertices[simplex] + normal = np.cross(v1 - v0, v2 - v0) + face_center = (v0 + v1 + v2) / 3 + to_face = face_center - centroid + if np.dot(normal, to_face) < 0: + corrected_simplices.append([simplex[0], simplex[2], simplex[1]]) + else: + corrected_simplices.append(simplex.tolist()) + indices = np.array(corrected_simplices).flatten().tolist() + + with DisableRendering(self.client): + col_shape_id = self.client.createCollisionShape( + pb.GEOM_MESH, + vertices = vertices.tolist(), + indices = indices, + ) + + vis_shape_id = self.client.createVisualShape( + pb.GEOM_MESH, + vertices = vertices.tolist(), + indices = indices, + rgbaColor = handle_color(name or polytope.name, color), + ) + + multibody_id = self.client.createMultiBody( + baseCollisionShapeIndex = col_shape_id, + baseVisualShapeIndex = vis_shape_id, + basePosition = [0, 0, 0], + ) + + display_name = name or polytope.name + if display_name: + centroid = vertices.mean(axis = 0) + self.client.addUserDebugText( + text = display_name, + textPosition = centroid.tolist(), + textColorRGB = [0., 0., 0.], + ) + + return multibody_id + def update_object_position( self, multibody_id: int, position: Position, rot_xywz: XYZWQuaternion = [0, 0, 0, 1] ):