From 53b7f8725a03c105f90abebc5071ea0c33341d06 Mon Sep 17 00:00:00 2001 From: Clayton Ramsey Date: Thu, 30 Apr 2026 15:24:47 -0500 Subject: [PATCH 1/3] feat: add Python access to Environment primitives --- src/impl/vamp/bindings/python/environment.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/impl/vamp/bindings/python/environment.cc b/src/impl/vamp/bindings/python/environment.cc index ae296b84..1c65dafc 100644 --- a/src/impl/vamp/bindings/python/environment.cc +++ b/src/impl/vamp/bindings/python/environment.cc @@ -1,3 +1,4 @@ +#include "vamp/collision/environment.hh" #include #include @@ -162,7 +163,14 @@ void vamp::binding::init_environment(nanobind::module_ &pymodule) .def( "attach", [](vc::Environment &e, const vc::Attachment &a) { e.attachments.emplace(a); }) - .def("detach", [](vc::Environment &e) { e.attachments.reset(); }); + .def("detach", [](vc::Environment &e) { e.attachments.reset(); }) + .def_ro("spheres", &vc::Environment::spheres) + .def_ro("cuboids", &vc::Environment::cuboids) + .def_ro("z_aligned_cuboids", &vc::Environment::z_aligned_cuboids) + .def_ro("capsules", &vc::Environment::capsules) + .def_ro("z_aligned_capsules", &vc::Environment::z_aligned_capsules) + .def_ro("heightfields", &vc::Environment::heightfields) + .def_ro("pointclouds", &vc::Environment::pointclouds); pymodule.def( "filter_pointcloud", From 8030bd9a3da29dfa7b7a32dea3d940cf50783ee9 Mon Sep 17 00:00:00 2001 From: Clayton Ramsey Date: Thu, 30 Apr 2026 15:29:08 -0500 Subject: [PATCH 2/3] chore: restyle includes for environment bindings --- src/impl/vamp/bindings/environment.cc | 236 ++++++++++++++++++++++++++ 1 file changed, 236 insertions(+) create mode 100644 src/impl/vamp/bindings/environment.cc diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc new file mode 100644 index 00000000..aa041b5d --- /dev/null +++ b/src/impl/vamp/bindings/environment.cc @@ -0,0 +1,236 @@ +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nb = nanobind; +namespace vc = vamp::collision; +namespace vf = vamp::collision::factory; +using namespace nb::literals; + +void vamp::binding::init_environment(nanobind::module_ &pymodule) +{ + nb::class_>(pymodule, "Sphere") + .def( + "__init__", + [](vc::Sphere *q, const std::array ¢er, float radius) noexcept + { new (q) vc::Sphere(vf::sphere::array(center, radius)); }, + "Constructor from center and radius.") + .def_static("make_flat", &vf::sphere::flat) + .def_static("make", &vf::sphere::array) + .def_ro("x", &vc::Sphere::x) + .def_ro("y", &vc::Sphere::y) + .def_ro("z", &vc::Sphere::z) + .def_ro("r", &vc::Sphere::r) + .def_prop_ro( + "position", + [](vc::Sphere &sphere) { return std::array{sphere.x, sphere.y, sphere.z}; }) + .def_ro("min_distance", &vc::Sphere::min_distance) + .def_rw("name", &vc::Sphere::name); + + nb::class_>(pymodule, "Cylinder") + .def( + "__init__", + [](vc::Cylinder *q, + const std::array ¢er, + const std::array &euler_xyz, + float radius, + float length) noexcept + { new (q) vc::Cylinder(vf::cylinder::center::array(center, euler_xyz, radius, length)); }, + "Constructor from center, Euler XYZ orientation, radius, and length.") + .def( + "__init__", + [](vc::Cylinder *q, + const std::array &endpoint1, + const std::array &endpoint2, + float radius) noexcept { *q = vf::cylinder::endpoints::array(endpoint1, endpoint2, radius); }, + "Constructor from endpoints and radius.") + .def_ro("x1", &vc::Cylinder::x1) + .def_ro("y1", &vc::Cylinder::y1) + .def_ro("z1", &vc::Cylinder::z1) + .def_prop_ro("x2", &vc::Cylinder::x2) + .def_prop_ro("y2", &vc::Cylinder::y2) + .def_prop_ro("z2", &vc::Cylinder::z2) + .def_ro("xv", &vc::Cylinder::xv) + .def_ro("yv", &vc::Cylinder::yv) + .def_ro("zv", &vc::Cylinder::zv) + .def_ro("r", &vc::Cylinder::r) + .def_ro("rdv", &vc::Cylinder::rdv) + .def_ro("min_distance", &vc::Cylinder::min_distance) + .def_rw("name", &vc::Cylinder::name); + + nb::class_>(pymodule, "Cuboid") + .def( + "__init__", + [](vc::Cuboid *q, + const std::array ¢er, + const std::array &euler_xyz, + const std::array &half_extents) noexcept + { new (q) vc::Cuboid(vf::cuboid::array(center, euler_xyz, half_extents)); }, + "Constructor from center, Euler XYZ orientation, and XYZ half-extents.") + .def_ro("x", &vc::Cuboid::x) + .def_ro("y", &vc::Cuboid::y) + .def_ro("z", &vc::Cuboid::z) + .def_ro("axis_1_x", &vc::Cuboid::axis_1_x) + .def_ro("axis_1_y", &vc::Cuboid::axis_1_y) + .def_ro("axis_1_z", &vc::Cuboid::axis_1_z) + .def_ro("axis_2_x", &vc::Cuboid::axis_2_x) + .def_ro("axis_2_y", &vc::Cuboid::axis_2_y) + .def_ro("axis_2_z", &vc::Cuboid::axis_2_z) + .def_ro("axis_3_x", &vc::Cuboid::axis_3_x) + .def_ro("axis_3_y", &vc::Cuboid::axis_3_y) + .def_ro("axis_3_z", &vc::Cuboid::axis_3_z) + .def_ro("axis_1_r", &vc::Cuboid::axis_1_r) + .def_ro("axis_2_r", &vc::Cuboid::axis_2_r) + .def_ro("axis_3_r", &vc::Cuboid::axis_3_r) + .def_ro("min_distance", &vc::Cuboid::min_distance) + .def_rw("name", &vc::Cuboid::name); + + pymodule.def("make_heightfield", &vf::heightfield::array); + + nb::class_>(pymodule, "HeightField") + .def_ro("x", &vc::HeightField::x) + .def_ro("y", &vc::HeightField::y) + .def_ro("z", &vc::HeightField::z) + .def_ro("xs", &vc::HeightField::xs) + .def_ro("ys", &vc::HeightField::ys) + .def_ro("zs", &vc::HeightField::zs) + .def_ro("data", &vc::HeightField::data); + + nb::class_>(pymodule, "Environment") + .def(nb::init<>()) + .def( + "add_sphere", + [](vc::Environment &e, const vc::Sphere &s) + { + e.spheres.emplace_back(s); + e.sort(); + }) + .def( + "add_cuboid", + [](vc::Environment &e, const vc::Cuboid &s) + { + if (s.axis_3_z == 1.) + { + e.z_aligned_cuboids.emplace_back(s); + } + else + { + e.cuboids.emplace_back(s); + } + e.sort(); + }) + .def( + "add_capsule", + [](vc::Environment &e, const vc::Cylinder &s) + { + if (s.xv == 0. and s.yv == 0.) + { + e.z_aligned_capsules.emplace_back(s); + } + else + { + e.capsules.emplace_back(s); + } + e.sort(); + }) + .def( + "add_heightfield", + [](vc::Environment &e, const vc::HeightField &s) + { e.heightfields.emplace_back(s); }) + .def( + "add_pointcloud", + [](vc::Environment &e, + const std::vector &pc, + float r_min, + float r_max, + float r_point) + { + auto start_time = std::chrono::steady_clock::now(); + e.pointclouds.emplace_back(pc, r_min, r_max, r_point); + return vamp::utils::get_elapsed_nanoseconds(start_time); + }) + .def( + "attach", + [](vc::Environment &e, const vc::Attachment &a) { e.attachments.emplace(a); }) + .def("detach", [](vc::Environment &e) { e.attachments.reset(); }) + .def_ro("spheres", &vc::Environment::spheres) + .def_ro("cuboids", &vc::Environment::cuboids) + .def_ro("z_aligned_cuboids", &vc::Environment::z_aligned_cuboids) + .def_ro("capsules", &vc::Environment::capsules) + .def_ro("z_aligned_capsules", &vc::Environment::z_aligned_capsules) + .def_ro("heightfields", &vc::Environment::heightfields) + .def_ro("pointclouds", &vc::Environment::pointclouds); + + pymodule.def( + "filter_pointcloud", + [](const std::vector &pc, + float min_dist, + float max_range, + const collision::Point &origin, + const collision::Point &workcell_min, + const collision::Point &workcell_max, + bool cull) -> std::pair, std::size_t> + { + auto start_time = std::chrono::steady_clock::now(); + auto filtered = + vc::filter_pointcloud(pc, min_dist, max_range, origin, workcell_min, workcell_max, cull); + return {filtered, vamp::utils::get_elapsed_nanoseconds(start_time)}; + }); + + pymodule.def( + "filter_pointcloud", + [](const nb::ndarray, nb::device::cpu> &pc, + float min_dist, + float max_range, + const collision::Point &origin, + const collision::Point &workcell_min, + const collision::Point &workcell_max, + bool cull) -> std::pair, std::size_t> + { + auto start_time = std::chrono::steady_clock::now(); + auto filtered = + vc::filter_pointcloud(pc, min_dist, max_range, origin, workcell_min, workcell_max, cull); + return {filtered, vamp::utils::get_elapsed_nanoseconds(start_time)}; + }); + + nb::class_>(pymodule, "Attachment") + .def( + "__init__", + [](vc::Attachment *q, Eigen::Matrix4f &tf) noexcept + { + Eigen::Isometry3f iso; + iso.matrix() = tf; + new (q) vc::Attachment(iso); + }, + "Constructor for an attachment centered at a relative transform from the end-effector.") + .def_prop_ro("relative_frame", [](vc::Attachment &a) { return a.tf; }) + .def( + "add_sphere", + [](vc::Attachment &a, collision::Sphere &sphere) + { a.spheres.emplace_back(sphere); }) + .def( + "add_spheres", + [](vc::Attachment &a, std::vector> &spheres) + { a.spheres.insert(a.spheres.end(), spheres.cbegin(), spheres.cend()); }) + .def( + "set_ee_pose", + [](vc::Attachment &a, Eigen::Matrix4f &tf) + { + Eigen::Isometry3f iso; + iso.matrix() = tf; + a.pose(iso); + }, + "tf"_a) + .def_ro("posed_spheres", &vc::Attachment::posed_spheres); +} From 6b1b9881dd85b2ed2d6c89f2815d6bbfb7206fa2 Mon Sep 17 00:00:00 2001 From: Clayton Ramsey Date: Thu, 30 Apr 2026 15:31:04 -0500 Subject: [PATCH 3/3] fix: do not reintroduce top-level bindings cc file --- src/impl/vamp/bindings/environment.cc | 236 ------------------- src/impl/vamp/bindings/python/environment.cc | 2 +- 2 files changed, 1 insertion(+), 237 deletions(-) delete mode 100644 src/impl/vamp/bindings/environment.cc diff --git a/src/impl/vamp/bindings/environment.cc b/src/impl/vamp/bindings/environment.cc deleted file mode 100644 index aa041b5d..00000000 --- a/src/impl/vamp/bindings/environment.cc +++ /dev/null @@ -1,236 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace nb = nanobind; -namespace vc = vamp::collision; -namespace vf = vamp::collision::factory; -using namespace nb::literals; - -void vamp::binding::init_environment(nanobind::module_ &pymodule) -{ - nb::class_>(pymodule, "Sphere") - .def( - "__init__", - [](vc::Sphere *q, const std::array ¢er, float radius) noexcept - { new (q) vc::Sphere(vf::sphere::array(center, radius)); }, - "Constructor from center and radius.") - .def_static("make_flat", &vf::sphere::flat) - .def_static("make", &vf::sphere::array) - .def_ro("x", &vc::Sphere::x) - .def_ro("y", &vc::Sphere::y) - .def_ro("z", &vc::Sphere::z) - .def_ro("r", &vc::Sphere::r) - .def_prop_ro( - "position", - [](vc::Sphere &sphere) { return std::array{sphere.x, sphere.y, sphere.z}; }) - .def_ro("min_distance", &vc::Sphere::min_distance) - .def_rw("name", &vc::Sphere::name); - - nb::class_>(pymodule, "Cylinder") - .def( - "__init__", - [](vc::Cylinder *q, - const std::array ¢er, - const std::array &euler_xyz, - float radius, - float length) noexcept - { new (q) vc::Cylinder(vf::cylinder::center::array(center, euler_xyz, radius, length)); }, - "Constructor from center, Euler XYZ orientation, radius, and length.") - .def( - "__init__", - [](vc::Cylinder *q, - const std::array &endpoint1, - const std::array &endpoint2, - float radius) noexcept { *q = vf::cylinder::endpoints::array(endpoint1, endpoint2, radius); }, - "Constructor from endpoints and radius.") - .def_ro("x1", &vc::Cylinder::x1) - .def_ro("y1", &vc::Cylinder::y1) - .def_ro("z1", &vc::Cylinder::z1) - .def_prop_ro("x2", &vc::Cylinder::x2) - .def_prop_ro("y2", &vc::Cylinder::y2) - .def_prop_ro("z2", &vc::Cylinder::z2) - .def_ro("xv", &vc::Cylinder::xv) - .def_ro("yv", &vc::Cylinder::yv) - .def_ro("zv", &vc::Cylinder::zv) - .def_ro("r", &vc::Cylinder::r) - .def_ro("rdv", &vc::Cylinder::rdv) - .def_ro("min_distance", &vc::Cylinder::min_distance) - .def_rw("name", &vc::Cylinder::name); - - nb::class_>(pymodule, "Cuboid") - .def( - "__init__", - [](vc::Cuboid *q, - const std::array ¢er, - const std::array &euler_xyz, - const std::array &half_extents) noexcept - { new (q) vc::Cuboid(vf::cuboid::array(center, euler_xyz, half_extents)); }, - "Constructor from center, Euler XYZ orientation, and XYZ half-extents.") - .def_ro("x", &vc::Cuboid::x) - .def_ro("y", &vc::Cuboid::y) - .def_ro("z", &vc::Cuboid::z) - .def_ro("axis_1_x", &vc::Cuboid::axis_1_x) - .def_ro("axis_1_y", &vc::Cuboid::axis_1_y) - .def_ro("axis_1_z", &vc::Cuboid::axis_1_z) - .def_ro("axis_2_x", &vc::Cuboid::axis_2_x) - .def_ro("axis_2_y", &vc::Cuboid::axis_2_y) - .def_ro("axis_2_z", &vc::Cuboid::axis_2_z) - .def_ro("axis_3_x", &vc::Cuboid::axis_3_x) - .def_ro("axis_3_y", &vc::Cuboid::axis_3_y) - .def_ro("axis_3_z", &vc::Cuboid::axis_3_z) - .def_ro("axis_1_r", &vc::Cuboid::axis_1_r) - .def_ro("axis_2_r", &vc::Cuboid::axis_2_r) - .def_ro("axis_3_r", &vc::Cuboid::axis_3_r) - .def_ro("min_distance", &vc::Cuboid::min_distance) - .def_rw("name", &vc::Cuboid::name); - - pymodule.def("make_heightfield", &vf::heightfield::array); - - nb::class_>(pymodule, "HeightField") - .def_ro("x", &vc::HeightField::x) - .def_ro("y", &vc::HeightField::y) - .def_ro("z", &vc::HeightField::z) - .def_ro("xs", &vc::HeightField::xs) - .def_ro("ys", &vc::HeightField::ys) - .def_ro("zs", &vc::HeightField::zs) - .def_ro("data", &vc::HeightField::data); - - nb::class_>(pymodule, "Environment") - .def(nb::init<>()) - .def( - "add_sphere", - [](vc::Environment &e, const vc::Sphere &s) - { - e.spheres.emplace_back(s); - e.sort(); - }) - .def( - "add_cuboid", - [](vc::Environment &e, const vc::Cuboid &s) - { - if (s.axis_3_z == 1.) - { - e.z_aligned_cuboids.emplace_back(s); - } - else - { - e.cuboids.emplace_back(s); - } - e.sort(); - }) - .def( - "add_capsule", - [](vc::Environment &e, const vc::Cylinder &s) - { - if (s.xv == 0. and s.yv == 0.) - { - e.z_aligned_capsules.emplace_back(s); - } - else - { - e.capsules.emplace_back(s); - } - e.sort(); - }) - .def( - "add_heightfield", - [](vc::Environment &e, const vc::HeightField &s) - { e.heightfields.emplace_back(s); }) - .def( - "add_pointcloud", - [](vc::Environment &e, - const std::vector &pc, - float r_min, - float r_max, - float r_point) - { - auto start_time = std::chrono::steady_clock::now(); - e.pointclouds.emplace_back(pc, r_min, r_max, r_point); - return vamp::utils::get_elapsed_nanoseconds(start_time); - }) - .def( - "attach", - [](vc::Environment &e, const vc::Attachment &a) { e.attachments.emplace(a); }) - .def("detach", [](vc::Environment &e) { e.attachments.reset(); }) - .def_ro("spheres", &vc::Environment::spheres) - .def_ro("cuboids", &vc::Environment::cuboids) - .def_ro("z_aligned_cuboids", &vc::Environment::z_aligned_cuboids) - .def_ro("capsules", &vc::Environment::capsules) - .def_ro("z_aligned_capsules", &vc::Environment::z_aligned_capsules) - .def_ro("heightfields", &vc::Environment::heightfields) - .def_ro("pointclouds", &vc::Environment::pointclouds); - - pymodule.def( - "filter_pointcloud", - [](const std::vector &pc, - float min_dist, - float max_range, - const collision::Point &origin, - const collision::Point &workcell_min, - const collision::Point &workcell_max, - bool cull) -> std::pair, std::size_t> - { - auto start_time = std::chrono::steady_clock::now(); - auto filtered = - vc::filter_pointcloud(pc, min_dist, max_range, origin, workcell_min, workcell_max, cull); - return {filtered, vamp::utils::get_elapsed_nanoseconds(start_time)}; - }); - - pymodule.def( - "filter_pointcloud", - [](const nb::ndarray, nb::device::cpu> &pc, - float min_dist, - float max_range, - const collision::Point &origin, - const collision::Point &workcell_min, - const collision::Point &workcell_max, - bool cull) -> std::pair, std::size_t> - { - auto start_time = std::chrono::steady_clock::now(); - auto filtered = - vc::filter_pointcloud(pc, min_dist, max_range, origin, workcell_min, workcell_max, cull); - return {filtered, vamp::utils::get_elapsed_nanoseconds(start_time)}; - }); - - nb::class_>(pymodule, "Attachment") - .def( - "__init__", - [](vc::Attachment *q, Eigen::Matrix4f &tf) noexcept - { - Eigen::Isometry3f iso; - iso.matrix() = tf; - new (q) vc::Attachment(iso); - }, - "Constructor for an attachment centered at a relative transform from the end-effector.") - .def_prop_ro("relative_frame", [](vc::Attachment &a) { return a.tf; }) - .def( - "add_sphere", - [](vc::Attachment &a, collision::Sphere &sphere) - { a.spheres.emplace_back(sphere); }) - .def( - "add_spheres", - [](vc::Attachment &a, std::vector> &spheres) - { a.spheres.insert(a.spheres.end(), spheres.cbegin(), spheres.cend()); }) - .def( - "set_ee_pose", - [](vc::Attachment &a, Eigen::Matrix4f &tf) - { - Eigen::Isometry3f iso; - iso.matrix() = tf; - a.pose(iso); - }, - "tf"_a) - .def_ro("posed_spheres", &vc::Attachment::posed_spheres); -} diff --git a/src/impl/vamp/bindings/python/environment.cc b/src/impl/vamp/bindings/python/environment.cc index 1c65dafc..aa041b5d 100644 --- a/src/impl/vamp/bindings/python/environment.cc +++ b/src/impl/vamp/bindings/python/environment.cc @@ -1,6 +1,6 @@ -#include "vamp/collision/environment.hh" #include +#include #include #include #include