From 002d4d192b3d6ad647f71c5a05b3109122844884 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 11 Dec 2025 23:37:05 -0600 Subject: [PATCH 01/10] Add validate_motion function --- src/impl/vamp/bindings/robot_helper.hh | 27 ++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 8fa3d7c1..ee0d9c44 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -266,6 +266,25 @@ namespace vamp::binding configuration, configuration, EnvironmentVector(environment)); } + inline static auto + validate_motion(const Type &c_in, const Type &c_out, const EnvironmentInput &environment, bool check_bounds = false) -> bool + { + auto configuration_in = Input::to(c_in); + auto copy_in = configuration_in.trim(); + Robot::descale_configuration(copy_in); + + const bool in_bounds_in = (copy_in <= 1.F).all() and (copy_in >= 0.F).all(); + + auto configuration_out = Input::to(c_out); + auto copy_out = configuration_out.trim(); + Robot::descale_configuration(copy_out); + + const bool in_bounds_out = (copy_out <= 1.F).all() and (copy_out >= 0.F).all(); + + return (not check_bounds or in_bounds_in or in_bounds_out) and + vamp::planning::validate_motion( + configuration_in, configuration_out, EnvironmentVector(environment)); + } inline static auto simplify( const Path &path, const EnvironmentInput &environment, @@ -552,6 +571,14 @@ namespace vamp::binding "environment"_a = vamp::collision::Environment(), "check_bounds"_a = false); + MF("validate_motion", + validate_motion, + "Check if a configuration is valid. Returns true if valid.", + "configuration_in"_a, + "configuration_out"_a, + "environment"_a = vamp::collision::Environment(), + "check_bounds"_a = false); + MF("filter_self_from_pointcloud", filter_self_from_pointcloud, "Removes points from pointcloud which collide with the robot and environment.", From 8f1ec78ca7f7698f51b327dd3e96ebb5af7c7e62 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Thu, 11 Dec 2025 23:37:42 -0600 Subject: [PATCH 02/10] Add upper and lower bounds functions --- src/impl/vamp/bindings/robot_helper.hh | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index ee0d9c44..07515ce8 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -375,7 +375,23 @@ namespace vamp::binding submodule.def( "joint_names", []() { return Robot::joint_names; }, "Joint names for the robot in order of DoF"); submodule.def("end_effector", []() { return Robot::end_effector; }, "End-effector frame name."); - + + submodule.def( + "upper_bounds", []() -> NDArray { + std::array ones; + ones.fill(1.0f); + auto one_v = typename Robot::Configuration(ones); + Robot::scale_configuration(one_v); + return NA::from(one_v); + }); + submodule.def( + "lower_bounds", []() -> NDArray { + std::array zeros; + zeros.fill(0.0f); + auto zero_v = typename Robot::Configuration(zeros); + Robot::scale_configuration(zero_v); + return NA::from(zero_v); + }); using RNG = vamp::rng::RNG; nb::class_(submodule, "RNG", "RNG for robot configurations.") .def( @@ -576,8 +592,8 @@ namespace vamp::binding "Check if a configuration is valid. Returns true if valid.", "configuration_in"_a, "configuration_out"_a, - "environment"_a = vamp::collision::Environment(), - "check_bounds"_a = false); + "environment"_a = vamp::collision::Environment(), + "check_bounds"_a = false); MF("filter_self_from_pointcloud", filter_self_from_pointcloud, From f2bd81ba25afe7fed5eb3c7120d0b20ac4d3c363 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 12 Dec 2025 09:06:32 -0600 Subject: [PATCH 03/10] Clang format --- src/impl/vamp/bindings/robot_helper.hh | 30 ++++++++++++++++---------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 07515ce8..05516c3b 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -266,8 +266,11 @@ namespace vamp::binding configuration, configuration, EnvironmentVector(environment)); } - inline static auto - validate_motion(const Type &c_in, const Type &c_out, const EnvironmentInput &environment, bool check_bounds = false) -> bool + inline static auto validate_motion( + const Type &c_in, + const Type &c_out, + const EnvironmentInput &environment, + bool check_bounds = false) -> bool { auto configuration_in = Input::to(c_in); auto copy_in = configuration_in.trim(); @@ -285,6 +288,7 @@ namespace vamp::binding vamp::planning::validate_motion( configuration_in, configuration_out, EnvironmentVector(environment)); } + inline static auto simplify( const Path &path, const EnvironmentInput &environment, @@ -375,9 +379,11 @@ namespace vamp::binding submodule.def( "joint_names", []() { return Robot::joint_names; }, "Joint names for the robot in order of DoF"); submodule.def("end_effector", []() { return Robot::end_effector; }, "End-effector frame name."); - + submodule.def( - "upper_bounds", []() -> NDArray { + "upper_bounds", + []() -> NDArray + { std::array ones; ones.fill(1.0f); auto one_v = typename Robot::Configuration(ones); @@ -385,7 +391,9 @@ namespace vamp::binding return NA::from(one_v); }); submodule.def( - "lower_bounds", []() -> NDArray { + "lower_bounds", + []() -> NDArray + { std::array zeros; zeros.fill(0.0f); auto zero_v = typename Robot::Configuration(zeros); @@ -588,12 +596,12 @@ namespace vamp::binding "check_bounds"_a = false); MF("validate_motion", - validate_motion, - "Check if a configuration is valid. Returns true if valid.", - "configuration_in"_a, - "configuration_out"_a, - "environment"_a = vamp::collision::Environment(), - "check_bounds"_a = false); + validate_motion, + "Check if a configuration is valid. Returns true if valid.", + "configuration_in"_a, + "configuration_out"_a, + "environment"_a = vamp::collision::Environment(), + "check_bounds"_a = false); MF("filter_self_from_pointcloud", filter_self_from_pointcloud, From 06063ca45d7ae1401bf3a7fb75ffa22b7a12d78c Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 12 Dec 2025 10:10:21 -0600 Subject: [PATCH 04/10] fix: update bounds checking logic --- src/impl/vamp/bindings/robot_helper.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 05516c3b..cd66c5e9 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -284,7 +284,7 @@ namespace vamp::binding const bool in_bounds_out = (copy_out <= 1.F).all() and (copy_out >= 0.F).all(); - return (not check_bounds or in_bounds_in or in_bounds_out) and + return (not check_bounds or (in_bounds_in and in_bounds_out)) and vamp::planning::validate_motion( configuration_in, configuration_out, EnvironmentVector(environment)); } From 8db1c51dc756c4af6884e7a3bcad677e6e45efc5 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Wed, 21 Jan 2026 16:33:11 -0600 Subject: [PATCH 05/10] Signed distance field implementation --- CMakeLists.txt | 14 ++ scripts/cpp/sdf_panda_example.cc | 171 ++++++++++++++++++++++ src/impl/vamp/collision/sphere_capsule.hh | 45 ++++++ src/impl/vamp/collision/sphere_cuboid.hh | 69 +++++++++ src/impl/vamp/collision/sphere_sphere.hh | 26 ++++ src/impl/vamp/collision/validity.hh | 47 ++++++ src/impl/vamp/robots/panda.hh | 22 +++ src/impl/vamp/robots/ur5.hh | 22 +++ 8 files changed, 416 insertions(+) create mode 100644 scripts/cpp/sdf_panda_example.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 1217634c..cb80fec4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,6 +146,20 @@ if(VAMP_BUILD_CPP_DEMO) if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") target_compile_options(vamp_rrtc_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) endif() + + add_executable(vamp_sdf_example scripts/cpp/sdf_example.cc) + target_link_libraries(vamp_sdf_example PRIVATE vamp_cpp) + + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(vamp_sdf_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) + endif() + + add_executable(vamp_sdf_panda_example scripts/cpp/sdf_panda_example.cc) + target_link_libraries(vamp_sdf_panda_example PRIVATE vamp_cpp) + + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(vamp_sdf_panda_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) + endif() endif() # OMPL integration demo diff --git a/scripts/cpp/sdf_panda_example.cc b/scripts/cpp/sdf_panda_example.cc new file mode 100644 index 00000000..70ca0ecc --- /dev/null +++ b/scripts/cpp/sdf_panda_example.cc @@ -0,0 +1,171 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +// Use Panda since we added the SDF method to it +using Robot = vamp::robots::Panda; +static constexpr const std::size_t rake = vamp::FloatVectorWidth; +using EnvironmentInput = vamp::collision::Environment; +using EnvironmentVector = vamp::collision::Environment>; + +// Spheres for the environment - (x, y, z) center coordinates +static const std::vector> obstacles = { + {0.55, 0, 0.25}, + {0.35, 0.35, 0.25}, + {0, 0.55, 0.25}, + {-0.55, 0, 0.25}, + {-0.35, -0.35, 0.25}, + {0, -0.55, 0.25}, + {0.35, -0.35, 0.25}, + {-0.55, 0.55, 0.25}, + + {0.35, 0.35, 0.8}, + {0, 0.55, 0.8}, + {-0.35, 0.35, 0.8}, + {-0.55, 0, 0.8}, + {-0.35, -0.35, 0.8}, + {0, -0.55, 0.8}, + {0.35, -0.35, 0.8}, + {0.55, 0, 0.8}, +}; + +// Radius for obstacle spheres +static constexpr float radius = 0.2; + +auto read_configs(const std::string& filename) -> std::vector +{ + std::vector configs; + std::ifstream file(filename); + + if (!file.is_open()) { + std::cerr << "Error: Could not open file " << filename << std::endl; + return configs; + } + + std::string line; + while (std::getline(file, line)) { + if (line.empty()) continue; + + std::stringstream ss(line); + Robot::ConfigurationArray config; + bool valid = true; + for (size_t i = 0; i < Robot::dimension; ++i) { + if (!(ss >> config[i])) { + valid = false; + break; + } + } + + if (valid) { + configs.push_back(config); + } + } + return configs; +} + +auto main(int, char **) -> int +{ + // Build environment + EnvironmentInput environment; + for (const auto &sphere : obstacles) + { + environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius)); + } + + // Sort environment for efficiency + environment.sort(); + auto env_v = EnvironmentVector(environment); + + // Read configurations + std::string config_file = "scripts/cpp/configs.txt"; + // Check if path exists relative to build dir or source + // Assuming run from project root, path is correct. + // If run from build, it might be ../scripts/cpp/configs.txt + // But user command is usually ./build/exe from root. + + auto all_configs = read_configs(config_file); + + if (all_configs.empty()) { + std::cerr << "No configurations loaded from " << config_file << std::endl; + return 1; + } + + // Open output file + std::string output_file = "scripts/cpp/sdf_results.txt"; + std::ofstream out_stream(output_file); + if (!out_stream.is_open()) { + std::cerr << "Error: Could not open output file " << output_file << std::endl; + return 1; + } + + std::cout << "Writing results to " << output_file << "..." << std::endl; + + // Process in batches of 'rake' + size_t num_configs = all_configs.size(); + for (size_t batch_idx = 0; batch_idx < num_configs; batch_idx += rake) + { + Robot::ConfigurationBlock q; + + // Fill the batch (by dimension first, then lane) + for (size_t j = 0; j < Robot::dimension; ++j) + { + std::array joint_vals; + for (size_t lane = 0; lane < rake; ++lane) + { + size_t config_idx = batch_idx + lane; + if (config_idx < num_configs) { + joint_vals[lane] = all_configs[config_idx][j]; + } else { + joint_vals[lane] = 0.0f; // Padding + } + } + q[j] = Robot::ConfigurationBlock::RowT(joint_vals); + } + + // 1. Compute Forward Kinematics to get sphere positions + Robot::Spheres spheres; + Robot::sphere_fk(q, spheres); + + // 2. Compute Signed Distance Field + auto dists = Robot::sdf(env_v, spheres); + std::array dists_arr = dists.to_array(); + + // Output results for valid configs in this batch + for (size_t lane = 0; lane < rake; ++lane) + { + size_t config_idx = batch_idx + lane; + if (config_idx >= num_configs) break; + + // Write to file: ConfigIndex MinSDF Sphere0_X Sphere0_Y Sphere0_Z Sphere0_R ... + out_stream << config_idx << " " << std::fixed << std::setprecision(6) << dists_arr[lane]; + + // Output sphere poses and radii for this config (lane) + // Spheres struct has FloatVector x, y, z, r which are arrays of vectors + // We need to access element 'lane' of each sphere 'i' + + // Extract sphere data for the current lane + for (size_t i = 0; i < Robot::n_spheres; ++i) { + float sx = spheres.x[i][std::pair(0, lane)]; // Row 0, Col 'lane' logic + float sy = spheres.y[i][std::pair(0, lane)]; + float sz = spheres.z[i][std::pair(0, lane)]; + float sr = spheres.r[i][std::pair(0, lane)]; + out_stream << " " << sx << " " << sy << " " << sz << " " << sr; + } + + out_stream << std::endl; + } + } + + out_stream.close(); + std::cout << "Done." << std::endl; + + return 0; +} diff --git a/src/impl/vamp/collision/sphere_capsule.hh b/src/impl/vamp/collision/sphere_capsule.hh index 7ee241ca..6c58e91b 100644 --- a/src/impl/vamp/collision/sphere_capsule.hh +++ b/src/impl/vamp/collision/sphere_capsule.hh @@ -49,4 +49,49 @@ namespace vamp::collision { return sphere_z_aligned_capsule(c, s.x, s.y, s.z, s.r); } + + template + inline constexpr auto sphere_capsule_l2( + const Capsule &c, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + auto dot = dot_3(x - c.x1, y - c.y1, z - c.z1, c.xv, c.yv, c.zv); + auto cdf = (dot * c.rdv).clamp(0.F, 1.F); + + auto sum = sql2_3(x, y, z, c.x1 + c.xv * cdf, c.y1 + c.yv * cdf, c.z1 + c.zv * cdf).sqrt(); + auto rs = r + c.r; + return sum - rs; + } + + template + inline constexpr auto sphere_capsule_l2(const Capsule &c, const Sphere &s) noexcept -> DataT + { + return sphere_capsule_l2(c, s.x, s.y, s.z, s.r); + } + + template + inline constexpr auto sphere_z_aligned_capsule_l2( + const Capsule &c, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + auto dot = (z - c.z1) * c.zv; + auto cdf = (dot * c.rdv).clamp(0.F, 1.F); + + auto sum = sql2_3(x, y, z, c.x1, c.y1, c.z1 + c.zv * cdf).sqrt(); + auto rs = r + c.r; + return sum - rs; + } + + template + inline constexpr auto sphere_z_aligned_capsule_l2(const Capsule &c, const Sphere &s) noexcept + -> DataT + { + return sphere_z_aligned_capsule_l2(c, s.x, s.y, s.z, s.r); + } } // namespace vamp::collision diff --git a/src/impl/vamp/collision/sphere_cuboid.hh b/src/impl/vamp/collision/sphere_cuboid.hh index 0ba6a045..f41d7a91 100644 --- a/src/impl/vamp/collision/sphere_cuboid.hh +++ b/src/impl/vamp/collision/sphere_cuboid.hh @@ -57,4 +57,73 @@ namespace vamp::collision { return sphere_z_aligned_cuboid(c, s.x, s.y, s.z, s.r * s.r); } + + template + inline constexpr auto sphere_cuboid_l2( + const Cuboid &c, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + auto xs = x - c.x; + auto ys = y - c.y; + auto zs = z - c.z; + + auto q1 = dot_3(c.axis_1_x, c.axis_1_y, c.axis_1_z, xs, ys, zs).abs() - c.axis_1_r; + auto q2 = dot_3(c.axis_2_x, c.axis_2_y, c.axis_2_z, xs, ys, zs).abs() - c.axis_2_r; + auto q3 = dot_3(c.axis_3_x, c.axis_3_y, c.axis_3_z, xs, ys, zs).abs() - c.axis_3_r; + + auto a1 = q1.max(0.); + auto a2 = q2.max(0.); + auto a3 = q3.max(0.); + + auto outside_dist = dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + + auto max_q = q1.max(q2).max(q3); + auto inside_dist = -((-max_q).max(0.)); + + return outside_dist + inside_dist - r; + } + + template + inline constexpr auto sphere_cuboid_l2(const Cuboid &c, const Sphere &s) noexcept -> DataT + { + return sphere_cuboid_l2(c, s.x, s.y, s.z, s.r); + } + + template + inline constexpr auto sphere_z_aligned_cuboid_l2( + const Cuboid &c, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + auto xs = x - c.x; + auto ys = y - c.y; + auto zs = z - c.z; + + auto q1 = dot_2(c.axis_1_x, c.axis_1_y, xs, ys).abs() - c.axis_1_r; + auto q2 = dot_2(c.axis_2_x, c.axis_2_y, xs, ys).abs() - c.axis_2_r; + auto q3 = zs.abs() - c.axis_3_r; + + auto a1 = q1.max(0.); + auto a2 = q2.max(0.); + auto a3 = q3.max(0.); + + auto outside_dist = dot_3(a1, a2, a3, a1, a2, a3).sqrt(); + + auto max_q = q1.max(q2).max(q3); + auto inside_dist = -((-max_q).max(0.)); + + return outside_dist + inside_dist - r; + } + + template + inline constexpr auto sphere_z_aligned_cuboid_l2(const Cuboid &c, const Sphere &s) noexcept + -> DataT + { + return sphere_z_aligned_cuboid_l2(c, s.x, s.y, s.z, s.r); + } } // namespace vamp::collision diff --git a/src/impl/vamp/collision/sphere_sphere.hh b/src/impl/vamp/collision/sphere_sphere.hh index 27739a0c..e3dceea3 100644 --- a/src/impl/vamp/collision/sphere_sphere.hh +++ b/src/impl/vamp/collision/sphere_sphere.hh @@ -39,6 +39,32 @@ namespace vamp::collision return sphere_sphere_sql2(a, b.x, b.y, b.z, b.r); } + template + inline constexpr auto sphere_sphere_l2( + const DataT &ax, + const DataT &ay, + const DataT &az, + const DataT &ar, + const DataT &bx, + const DataT &by, + const DataT &bz, + const DataT &br) noexcept -> DataT + { + auto sum = sql2_3(ax, ay, az, bx, by, bz).sqrt(); + return sum - (ar + br); + } + + template + inline constexpr auto sphere_sphere_l2( + const Sphere &a, + const DataT &x, + const DataT &y, + const DataT &z, + const DataT &r) noexcept -> DataT + { + return sphere_sphere_l2(a.x, a.y, a.z, a.r, x, y, z, r); + } + template inline constexpr auto sphere_sphere_l2(const Sphere &a, const Sphere &b) noexcept -> DataT { diff --git a/src/impl/vamp/collision/validity.hh b/src/impl/vamp/collision/validity.hh index b555c13e..cfd27bcf 100644 --- a/src/impl/vamp/collision/validity.hh +++ b/src/impl/vamp/collision/validity.hh @@ -149,6 +149,53 @@ namespace vamp return false; } + template + inline auto sphere_environment_sdf( + const collision::Environment &e, + ArgT1 sx_, + ArgT2 sy_, + ArgT3 sz_, + ArgT4 sr_) noexcept -> DataT + { + auto sx = static_cast(sx_); + auto sy = static_cast(sy_); + auto sz = static_cast(sz_); + auto sr = static_cast(sr_); + + auto min_dist = static_cast(1.0e30); + + auto update_min = [&](const DataT &d) { + min_dist = -((-min_dist).max(-d)); + }; + + for (const auto &es : e.spheres) + { + update_min(collision::sphere_sphere_l2(es, sx, sy, sz, sr)); + } + + for (const auto &ec : e.capsules) + { + update_min(collision::sphere_capsule_l2(ec, sx, sy, sz, sr)); + } + + for (const auto &ec : e.z_aligned_capsules) + { + update_min(collision::sphere_z_aligned_capsule_l2(ec, sx, sy, sz, sr)); + } + + for (const auto &ec : e.cuboids) + { + update_min(collision::sphere_cuboid_l2(ec, sx, sy, sz, sr)); + } + + for (const auto &ec : e.z_aligned_cuboids) + { + update_min(collision::sphere_z_aligned_cuboid_l2(ec, sx, sy, sz, sr)); + } + + return min_dist; + } + template inline auto sphere_environment_get_collisions( const collision::Environment &e, // diff --git a/src/impl/vamp/robots/panda.hh b/src/impl/vamp/robots/panda.hh index 0c51e3f9..83e282d7 100644 --- a/src/impl/vamp/robots/panda.hh +++ b/src/impl/vamp/robots/panda.hh @@ -461,6 +461,28 @@ namespace vamp::robots } } + template + inline static auto sdf( + const vamp::collision::Environment> &environment, + const Spheres &spheres) noexcept -> FloatVector + { + auto min_dist = FloatVector::fill(1.0e30f); + + for (std::size_t i = 0; i < n_spheres; ++i) + { + auto d = vamp::sphere_environment_sdf( + environment, + spheres.x[i], + spheres.y[i], + spheres.z[i], + spheres.r[i]); + + min_dist = -((-min_dist).max(-d)); + } + + return min_dist; + } + using Debug = std:: pair>, std::vector>>; diff --git a/src/impl/vamp/robots/ur5.hh b/src/impl/vamp/robots/ur5.hh index 5cc815c7..587b28d0 100644 --- a/src/impl/vamp/robots/ur5.hh +++ b/src/impl/vamp/robots/ur5.hh @@ -397,6 +397,28 @@ namespace vamp::robots } } + template + inline static auto sdf( + const vamp::collision::Environment> &environment, + const Spheres &spheres) noexcept -> FloatVector + { + auto min_dist = FloatVector::fill(1.0e30); + + for (std::size_t i = 0; i < n_spheres; ++i) + { + auto d = vamp::sphere_environment_sdf( + environment, + spheres.x[i], + spheres.y[i], + spheres.z[i], + spheres.r[i]); + + min_dist = -((-min_dist).max(-d)); + } + + return min_dist; + } + using Debug = std:: pair>, std::vector>>; From 16842d57594ce868fb79e8724eac1d4b1981d3b5 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 23 Jan 2026 12:47:19 -0600 Subject: [PATCH 06/10] Add sdf function and binding --- src/impl/vamp/bindings/robot_helper.hh | 12 ++++++++++++ src/impl/vamp/robots/baxter.hh | 23 +++++++++++++++++++++++ src/impl/vamp/robots/fetch.hh | 23 +++++++++++++++++++++++ src/impl/vamp/robots/panda.hh | 5 ++++- src/impl/vamp/robots/sphere.hh | 8 ++++++++ src/impl/vamp/robots/ur5.hh | 5 ++++- 6 files changed, 74 insertions(+), 2 deletions(-) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index cd66c5e9..f8fef5c0 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -304,6 +304,12 @@ namespace vamp::binding return Robot::eefk(Input::array(start)).matrix(); } + inline static auto sdf(const Type &c_in, const EnvironmentInput &environment) -> float + { + return Robot::template sdf(EnvironmentVector(environment), Input::template block(c_in)) + .to_array()[0]; + } + inline static auto filter_self_from_pointcloud( const std::vector &pc, float point_radius, @@ -603,6 +609,12 @@ namespace vamp::binding "environment"_a = vamp::collision::Environment(), "check_bounds"_a = false); + MF("sdf", + sdf, + "Compute the signed distance field (SDF) for a configuration.", + "configuration"_a, + "environment"_a = vamp::collision::Environment()); + MF("filter_self_from_pointcloud", filter_self_from_pointcloud, "Removes points from pointcloud which collide with the robot and environment.", diff --git a/src/impl/vamp/robots/baxter.hh b/src/impl/vamp/robots/baxter.hh index 5e9476ca..19321daa 100644 --- a/src/impl/vamp/robots/baxter.hh +++ b/src/impl/vamp/robots/baxter.hh @@ -688,6 +688,29 @@ namespace vamp::robots } } + template + inline static auto sdf( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + { + Spheres spheres; + sphere_fk(x, spheres); + + auto min_dist = FloatVector::fill(1.0e30); + + auto update_min = [&](const FloatVector &d) { + min_dist = -((-min_dist).max(-d)); + }; + + for (auto i = 0U; i < n_spheres; ++i) + { + update_min(sphere_environment_sdf( + environment, spheres.x[i], spheres.y[i], spheres.z[i], spheres.r[i])); + } + + return min_dist; + } + using Debug = std:: pair>, std::vector>>; diff --git a/src/impl/vamp/robots/fetch.hh b/src/impl/vamp/robots/fetch.hh index b7c4eb98..553d62d7 100644 --- a/src/impl/vamp/robots/fetch.hh +++ b/src/impl/vamp/robots/fetch.hh @@ -645,6 +645,29 @@ namespace vamp::robots } } + template + inline static auto sdf( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &x) noexcept + { + Spheres spheres; + sphere_fk(x, spheres); + + auto min_dist = FloatVector::fill(1.0e30); + + auto update_min = [&](const FloatVector &d) { + min_dist = -((-min_dist).max(-d)); + }; + + for (auto i = 0U; i < n_spheres; ++i) + { + update_min(sphere_environment_sdf( + environment, spheres.x[i], spheres.y[i], spheres.z[i], spheres.r[i])); + } + + return min_dist; + } + using Debug = std:: pair>, std::vector>>; diff --git a/src/impl/vamp/robots/panda.hh b/src/impl/vamp/robots/panda.hh index 83e282d7..7b7acadb 100644 --- a/src/impl/vamp/robots/panda.hh +++ b/src/impl/vamp/robots/panda.hh @@ -464,8 +464,11 @@ namespace vamp::robots template inline static auto sdf( const vamp::collision::Environment> &environment, - const Spheres &spheres) noexcept -> FloatVector + const ConfigurationBlock &x) noexcept -> FloatVector { + Spheres spheres; + sphere_fk(x, spheres); + auto min_dist = FloatVector::fill(1.0e30f); for (std::size_t i = 0; i < n_spheres; ++i) diff --git a/src/impl/vamp/robots/sphere.hh b/src/impl/vamp/robots/sphere.hh index 76383995..30c02c03 100644 --- a/src/impl/vamp/robots/sphere.hh +++ b/src/impl/vamp/robots/sphere.hh @@ -107,6 +107,14 @@ namespace vamp::robots return not sphere_environment_in_collision(environment, q[0], q[1], q[2], radius); } + template + static auto sdf( + const vamp::collision::Environment> &environment, + const ConfigurationBlock &q) noexcept + { + return sphere_environment_sdf(environment, q[0], q[1], q[2], radius); + } + using Debug = std:: pair>, std::vector>>; diff --git a/src/impl/vamp/robots/ur5.hh b/src/impl/vamp/robots/ur5.hh index 587b28d0..8490ad49 100644 --- a/src/impl/vamp/robots/ur5.hh +++ b/src/impl/vamp/robots/ur5.hh @@ -400,8 +400,11 @@ namespace vamp::robots template inline static auto sdf( const vamp::collision::Environment> &environment, - const Spheres &spheres) noexcept -> FloatVector + const ConfigurationBlock &x) noexcept -> FloatVector { + Spheres spheres; + sphere_fk(x, spheres); + auto min_dist = FloatVector::fill(1.0e30); for (std::size_t i = 0; i < n_spheres; ++i) From afce0391c371f056645db8740e399184b5d40431 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 23 Jan 2026 13:02:36 -0600 Subject: [PATCH 07/10] Add a solver to project the config to safe config --- CMakeLists.txt | 7 -- scripts/cpp/sdf_example.cc | 129 ++++++++++++++++++++++ scripts/cpp/sdf_panda_example.cc | 171 ------------------------------ src/impl/vamp/optimization/sdf.hh | 105 ++++++++++++++++++ 4 files changed, 234 insertions(+), 178 deletions(-) create mode 100644 scripts/cpp/sdf_example.cc delete mode 100644 scripts/cpp/sdf_panda_example.cc create mode 100644 src/impl/vamp/optimization/sdf.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index cb80fec4..d2d4fb6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -153,13 +153,6 @@ if(VAMP_BUILD_CPP_DEMO) if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") target_compile_options(vamp_sdf_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) endif() - - add_executable(vamp_sdf_panda_example scripts/cpp/sdf_panda_example.cc) - target_link_libraries(vamp_sdf_panda_example PRIVATE vamp_cpp) - - if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - target_compile_options(vamp_sdf_panda_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare) - endif() endif() # OMPL integration demo diff --git a/scripts/cpp/sdf_example.cc b/scripts/cpp/sdf_example.cc new file mode 100644 index 00000000..4a1021a5 --- /dev/null +++ b/scripts/cpp/sdf_example.cc @@ -0,0 +1,129 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Use Panda as in rrtc_example.cc +using Robot = vamp::robots::Panda; +static constexpr const std::size_t rake = vamp::FloatVectorWidth; +using EnvironmentInput = vamp::collision::Environment; +using EnvironmentVector = vamp::collision::Environment>; + +// Environment Setup from rrtc_example.cc +static const std::vector> problem = { + {0.55, 0, 0.25}, + {0.35, 0.35, 0.25}, + {0, 0.55, 0.25}, + {-0.55, 0, 0.25}, + {-0.35, -0.35, 0.25}, + {0, -0.55, 0.25}, + {0.35, -0.35, 0.25}, + {0.35, 0.35, 0.8}, + {0, 0.55, 0.8}, + {-0.35, 0.35, 0.8}, + {-0.55, 0, 0.8}, + {-0.35, -0.35, 0.8}, + {0, -0.55, 0.8}, + {0.35, -0.35, 0.8}, +}; + +static constexpr float radius = 0.2; + +// Benchmark Helper +template +double benchmark(std::string name, int iterations, Func func) +{ + auto start = std::chrono::high_resolution_clock::now(); + for (int i = 0; i < iterations; ++i) { + func(i); + } + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + double avg_time = duration.count() / iterations; + std::cout << name << ": " << avg_time << " ms/iter (Total: " << duration.count() << " ms)" << std::endl; + return avg_time; +} + +auto main(int, char **) -> int +{ + std::cout << "Initializing Benchmark..." << std::endl; + + // 1. Build Environment + EnvironmentInput environment; + for (const auto &sphere : problem) + { + environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius)); + } + environment.sort(); + auto env_v = EnvironmentVector(environment); + + // 2. Generate Random Configurations + static constexpr int N_SAMPLES = 1000; + std::vector configs; + auto rng = std::make_shared>(); + + // Discard first few samples (Halton) + for(int i=0; i<100; ++i) rng->next(); + + for(int i=0; inext() returns Configuration (wrapped) + configs.push_back(rng->next()); + } + + std::cout << "Generated " << N_SAMPLES << " random configurations." << std::endl; + std::cout << "Running benchmarks with " << N_SAMPLES << " samples..." << std::endl; + std::cout << "Note: Each sample is broadcasted to " << rake << " lanes for SIMD ops." << std::endl; + std::cout << "--------------------------------------------------" << std::endl; + + // 3. Benchmark: SDF Only + benchmark("SDF Only", N_SAMPLES, [&](int idx) { + // Broadcast single config to block + Robot::ConfigurationBlock block; + auto& cfg = configs[idx]; + for (size_t d = 0; d < Robot::dimension; ++d) { + std::array row; + row.fill(cfg.element(d)); + block[d] = Robot::ConfigurationBlock::RowT(row.data(), false); + } + + auto dists = Robot::sdf(env_v, block); + // Ensure not optimized away + volatile float val = dists.to_array()[0]; + (void)val; + }); + + // 4. Benchmark: Solver (10 steps) + benchmark("Solver (10 steps)", N_SAMPLES, [&](int idx) { + auto valid_block = vamp::optimization::project_to_valid( + configs[idx], + env_v, + 10, // steps + 0.5f, // learning rate + 0.05f // noise + ); + volatile float val = valid_block[0].to_array()[0]; + (void)val; + }); + + // 5. Benchmark: Solver (100 steps) + benchmark("Solver (100 steps)", N_SAMPLES, [&](int idx) { + auto valid_block = vamp::optimization::project_to_valid( + configs[idx], + env_v, + 100, // steps + 0.5f, // learning rate + 0.05f // noise + ); + volatile float val = valid_block[0].to_array()[0]; + (void)val; + }); + + return 0; +} diff --git a/scripts/cpp/sdf_panda_example.cc b/scripts/cpp/sdf_panda_example.cc deleted file mode 100644 index 70ca0ecc..00000000 --- a/scripts/cpp/sdf_panda_example.cc +++ /dev/null @@ -1,171 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -// Use Panda since we added the SDF method to it -using Robot = vamp::robots::Panda; -static constexpr const std::size_t rake = vamp::FloatVectorWidth; -using EnvironmentInput = vamp::collision::Environment; -using EnvironmentVector = vamp::collision::Environment>; - -// Spheres for the environment - (x, y, z) center coordinates -static const std::vector> obstacles = { - {0.55, 0, 0.25}, - {0.35, 0.35, 0.25}, - {0, 0.55, 0.25}, - {-0.55, 0, 0.25}, - {-0.35, -0.35, 0.25}, - {0, -0.55, 0.25}, - {0.35, -0.35, 0.25}, - {-0.55, 0.55, 0.25}, - - {0.35, 0.35, 0.8}, - {0, 0.55, 0.8}, - {-0.35, 0.35, 0.8}, - {-0.55, 0, 0.8}, - {-0.35, -0.35, 0.8}, - {0, -0.55, 0.8}, - {0.35, -0.35, 0.8}, - {0.55, 0, 0.8}, -}; - -// Radius for obstacle spheres -static constexpr float radius = 0.2; - -auto read_configs(const std::string& filename) -> std::vector -{ - std::vector configs; - std::ifstream file(filename); - - if (!file.is_open()) { - std::cerr << "Error: Could not open file " << filename << std::endl; - return configs; - } - - std::string line; - while (std::getline(file, line)) { - if (line.empty()) continue; - - std::stringstream ss(line); - Robot::ConfigurationArray config; - bool valid = true; - for (size_t i = 0; i < Robot::dimension; ++i) { - if (!(ss >> config[i])) { - valid = false; - break; - } - } - - if (valid) { - configs.push_back(config); - } - } - return configs; -} - -auto main(int, char **) -> int -{ - // Build environment - EnvironmentInput environment; - for (const auto &sphere : obstacles) - { - environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius)); - } - - // Sort environment for efficiency - environment.sort(); - auto env_v = EnvironmentVector(environment); - - // Read configurations - std::string config_file = "scripts/cpp/configs.txt"; - // Check if path exists relative to build dir or source - // Assuming run from project root, path is correct. - // If run from build, it might be ../scripts/cpp/configs.txt - // But user command is usually ./build/exe from root. - - auto all_configs = read_configs(config_file); - - if (all_configs.empty()) { - std::cerr << "No configurations loaded from " << config_file << std::endl; - return 1; - } - - // Open output file - std::string output_file = "scripts/cpp/sdf_results.txt"; - std::ofstream out_stream(output_file); - if (!out_stream.is_open()) { - std::cerr << "Error: Could not open output file " << output_file << std::endl; - return 1; - } - - std::cout << "Writing results to " << output_file << "..." << std::endl; - - // Process in batches of 'rake' - size_t num_configs = all_configs.size(); - for (size_t batch_idx = 0; batch_idx < num_configs; batch_idx += rake) - { - Robot::ConfigurationBlock q; - - // Fill the batch (by dimension first, then lane) - for (size_t j = 0; j < Robot::dimension; ++j) - { - std::array joint_vals; - for (size_t lane = 0; lane < rake; ++lane) - { - size_t config_idx = batch_idx + lane; - if (config_idx < num_configs) { - joint_vals[lane] = all_configs[config_idx][j]; - } else { - joint_vals[lane] = 0.0f; // Padding - } - } - q[j] = Robot::ConfigurationBlock::RowT(joint_vals); - } - - // 1. Compute Forward Kinematics to get sphere positions - Robot::Spheres spheres; - Robot::sphere_fk(q, spheres); - - // 2. Compute Signed Distance Field - auto dists = Robot::sdf(env_v, spheres); - std::array dists_arr = dists.to_array(); - - // Output results for valid configs in this batch - for (size_t lane = 0; lane < rake; ++lane) - { - size_t config_idx = batch_idx + lane; - if (config_idx >= num_configs) break; - - // Write to file: ConfigIndex MinSDF Sphere0_X Sphere0_Y Sphere0_Z Sphere0_R ... - out_stream << config_idx << " " << std::fixed << std::setprecision(6) << dists_arr[lane]; - - // Output sphere poses and radii for this config (lane) - // Spheres struct has FloatVector x, y, z, r which are arrays of vectors - // We need to access element 'lane' of each sphere 'i' - - // Extract sphere data for the current lane - for (size_t i = 0; i < Robot::n_spheres; ++i) { - float sx = spheres.x[i][std::pair(0, lane)]; // Row 0, Col 'lane' logic - float sy = spheres.y[i][std::pair(0, lane)]; - float sz = spheres.z[i][std::pair(0, lane)]; - float sr = spheres.r[i][std::pair(0, lane)]; - out_stream << " " << sx << " " << sy << " " << sz << " " << sr; - } - - out_stream << std::endl; - } - } - - out_stream.close(); - std::cout << "Done." << std::endl; - - return 0; -} diff --git a/src/impl/vamp/optimization/sdf.hh b/src/impl/vamp/optimization/sdf.hh new file mode 100644 index 00000000..0d1a8fab --- /dev/null +++ b/src/impl/vamp/optimization/sdf.hh @@ -0,0 +1,105 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace vamp::optimization +{ + // Computes Gradient of SDF via Central Difference + template + inline auto compute_gradient( + const collision::Environment> &environment, + const typename Robot::template ConfigurationBlock &state, + float h = 1e-4f) noexcept -> typename Robot::template ConfigurationBlock + { + using ConfigBlock = typename Robot::template ConfigurationBlock; + + ConfigBlock grad; + auto h_vec = FloatVector::fill(h); + auto inv_2h = FloatVector::fill(1.0f / (2.0f * h)); + + // Create a mutable copy of state to perturb + ConfigBlock perturbed = state; + + for (std::size_t i = 0; i < Robot::dimension; ++i) + { + auto original_val = perturbed[i]; + + // f(x + h) + perturbed[i] = original_val + h_vec; + auto f_plus = Robot::sdf(environment, perturbed); + + // f(x - h) + perturbed[i] = original_val - h_vec; + auto f_minus = Robot::sdf(environment, perturbed); + + // Restore original + perturbed[i] = original_val; + + // g = (f_plus - f_minus) / 2h + grad[i] = (f_plus - f_minus) * inv_2h; + } + return grad; + } + + // Projects a single state to multiple valid candidates. + template + inline auto project_to_valid( + const typename Robot::Configuration &start_state, + const collision::Environment> &environment, + int steps = 100, + float learning_rate = 0.5f, + float noise_scale = 0.1f) noexcept -> typename Robot::template ConfigurationBlock + { + using ConfigBlock = typename Robot::template ConfigurationBlock; + using Vector = FloatVector; + + ConfigBlock current_state; + + // 1. Initialization: Broadcast and Add Noise + // seeding with a fixed value for reproducibility, or use std::random_device{} + static std::random_device gen; + std::uniform_real_distribution dist_noise(-noise_scale, noise_scale); + + for (std::size_t i = 0; i < Robot::dimension; ++i) + { + // Create an array for the row + std::array noise_vals; + for (std::size_t k = 0; k < rake; ++k) + { + noise_vals[k] = start_state.element(i) + dist_noise(gen); + } + // Load into the SIMD vector for dimension i + current_state[i] = Vector(noise_vals.data(), false); + } + + auto zero = Vector::fill(0.0f); + auto lr = Vector::fill(learning_rate); + + for (int step = 0; step < steps; ++step) + { + // Calculate SDF + auto dist = Robot::sdf(environment, current_state); + + // Calculate Gradient + auto grad = compute_gradient(environment, current_state); + + // Update Rule + auto neg_dist = -dist; + auto relu_neg_dist = neg_dist.max(zero); + auto magnitude = lr * relu_neg_dist; + + // Apply update per dimension + for (std::size_t i = 0; i < Robot::dimension; ++i) + { + auto delta = grad[i] * magnitude; + current_state[i] = current_state[i] + delta; + } + } + + return current_state; + } +} From 585c5707ed6dabcfda5cfe9416b3900b9f33e3de Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 23 Jan 2026 13:08:45 -0600 Subject: [PATCH 08/10] Add convergence analysis --- scripts/cpp/sdf_example.cc | 56 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/scripts/cpp/sdf_example.cc b/scripts/cpp/sdf_example.cc index 4a1021a5..e5c9c2e8 100644 --- a/scripts/cpp/sdf_example.cc +++ b/scripts/cpp/sdf_example.cc @@ -125,5 +125,61 @@ auto main(int, char **) -> int (void)val; }); + std::cout << "\n--------------------------------------------------" << std::endl; + std::cout << "Convergence Analysis:" << std::endl; + std::cout << "--------------------------------------------------" << std::endl; + + auto analyze_convergence = [&](std::string label, int steps) { + int valid_lanes = 0; + int total_lanes = 0; + double total_sdf = 0.0; + double min_sdf = 1e9; + double max_sdf = -1e9; + + for(int idx = 0; idx < N_SAMPLES; ++idx) { + Robot::ConfigurationBlock block; + + if (steps == -1) { // Raw samples (no noise, no solver) + auto& cfg = configs[idx]; + for (size_t d = 0; d < Robot::dimension; ++d) { + std::array row; + row.fill(cfg.element(d)); + block[d] = Robot::ConfigurationBlock::RowT(row.data(), false); + } + } else { + block = vamp::optimization::project_to_valid( + configs[idx], + env_v, + steps, + 0.5f, + 0.05f + ); + } + + auto dists = Robot::sdf(env_v, block); + auto dists_arr = dists.to_array(); + + for(float d : dists_arr) { + if(d > 0) valid_lanes++; + total_lanes++; + total_sdf += d; + if(d < min_sdf) min_sdf = d; + if(d > max_sdf) max_sdf = d; + } + } + + double valid_rate = 100.0 * valid_lanes / total_lanes; + double avg_sdf = total_sdf / total_lanes; + + std::cout << std::left << std::setw(20) << label + << " | Valid Rate: " << std::fixed << std::setprecision(1) << valid_rate << "%" + << " | Avg SDF: " << std::setprecision(4) << avg_sdf + << " | Range: [" << min_sdf << ", " << max_sdf << "]" << std::endl; + }; + + analyze_convergence("Initial (Raw)", -1); + analyze_convergence("Solver (10 steps)", 10); + analyze_convergence("Solver (100 steps)", 100); + return 0; } From b926cc2dab57510ef49feda8c53405c4d1c51106 Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 23 Jan 2026 13:35:11 -0600 Subject: [PATCH 09/10] Add projection binding and example --- scripts/evaluate_sdf.py | 115 +++++++++++++++++++++++++ src/impl/vamp/bindings/robot_helper.hh | 41 +++++++++ 2 files changed, 156 insertions(+) create mode 100644 scripts/evaluate_sdf.py diff --git a/scripts/evaluate_sdf.py b/scripts/evaluate_sdf.py new file mode 100644 index 00000000..02f7708c --- /dev/null +++ b/scripts/evaluate_sdf.py @@ -0,0 +1,115 @@ +import numpy as np +import vamp +import time +from fire import Fire + +def main(robot_name: str = "panda", n_samples: int = 10000): + # Load robot module + if not hasattr(vamp, robot_name): + print(f"Robot {robot_name} not found in vamp.") + available_robots = [attr for attr in dir(vamp) if hasattr(getattr(vamp, attr), "sdf")] + print(f"Available robots: {available_robots}") + return + + robot = getattr(vamp, robot_name) + + # Create environment with some obstacles + env = vamp.Environment() + + # Add some spheres to create a non-trivial environment + obstacles = [ + ([0.5, 0.0, 0.5], 0.2), + ([0.0, 0.5, 0.5], 0.2), + ([0.5, 0.5, 0.5], 0.2), + ([0.3, -0.3, 0.3], 0.15), + ([-0.3, 0.3, 0.3], 0.15), + ([0.6, 0.0, 0.2], 0.1), + ] + + for center, radius in obstacles: + env.add_sphere(vamp.Sphere(center, radius)) + + print(f"Evaluating SDF for robot: {robot_name}") + print(f"Environment: {len(obstacles)} spheres") + + # Initialize RNG + rng = robot.halton() + rng.reset() + + sdf_values = [] + + print(f"Sampling {n_samples} configurations...") + start_time = time.time() + + for i in range(n_samples): + q = rng.next() + # Compute SDF + # Positive means outside (safe), Negative means inside (collision) + dist = robot.sdf(q, env) + sdf_values.append(dist) + + end_time = time.time() + duration = end_time - start_time + + sdf_values = np.array(sdf_values) + + print("-" * 30) + print(f"Results for {n_samples} queries:") + print(f"Total Time: {duration:.4f} s") + print(f"Average Time: {duration/n_samples*1e6:.2f} µs/query") + print(f"Throughput: {n_samples/duration:.2f} queries/s") + print("-" * 30) + print(f"SDF Statistics:") + print(f" Min Dist: {np.min(sdf_values):.4f}") + print(f" Max Dist: {np.max(sdf_values):.4f}") + print(f" Mean Dist: {np.mean(sdf_values):.4f}") + print(f" Std Dev: {np.std(sdf_values):.4f}") + print("-" * 30) + + n_collisions = np.sum(sdf_values < 0) + print(f"Collisions: {n_collisions} ({n_collisions/n_samples*100:.2f}%)") + print(f"Safe Configs: {n_samples - n_collisions}") + + # Benchmark Solver + print("\n" + "=" * 30) + print("Benchmarking Solver (project_to_valid)...") + + def benchmark_solver(steps): + print(f"\nRunning Solver with {steps} steps...") + valid_count = 0 + total_time = 0 + + # Use a subset of samples to save time if needed, but let's do all + # To strictly measure solver time, we measure the call + + start_t = time.time() + for i in range(n_samples // 10): # Run on 10% of samples to be quick, or full? Let's do 1000. + if i >= 1000: break + + # Re-generate to ensure randomness or reuse? Let's reuse configs from a list if we stored them, + # but we didn't store them all. + # Let's just generate new ones or use a block. + # Ideally we want to see if it fixes collisions. + + q_init = rng.next() # New random sample + + # project_to_valid returns a list of valid configurations (or candidates) + # C++ signature: returns std::vector + candidates = robot.project_to_valid(q_init, env, steps=steps, learning_rate=0.5, noise_scale=0.1) + + if len(candidates) > 0: + valid_count += 1 + + end_t = time.time() + elapsed = end_t - start_t + n_bench = min(n_samples // 10, 1000) + + print(f" Time for {n_bench} calls: {elapsed:.4f} s") + print(f" Avg Time: {elapsed/n_bench*1e3:.4f} ms/call") + print(f" Success Rate (returned >=1 candidates): {valid_count}/{n_bench} ({valid_count/n_bench*100:.1f}%)") + + benchmark_solver(10) + benchmark_solver(100) + +if __name__ == "__main__": + Fire(main) diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index f8fef5c0..42142ce3 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -348,6 +349,37 @@ namespace vamp::binding } return filtered; + return filtered; + } + + inline static auto project_to_valid( + const Type &c_in, + const EnvironmentInput &environment, + int steps = 10, + float learning_rate = 0.5f, + float noise_scale = 0.1f) -> std::vector + { + auto result_block = vamp::optimization::project_to_valid( + Input::to(c_in), + EnvironmentVector(environment), + steps, + learning_rate, + noise_scale); + + std::vector result; + result.reserve(rake); + + for (auto i = 0U; i < rake; ++i) + { + typename Robot::Configuration cfg; + for (auto d = 0U; d < Robot::dimension; ++d) + { + cfg[d] = result_block[d].element(i); + } + result.emplace_back(Input::from(cfg)); + } + + return result; } }; @@ -623,6 +655,15 @@ namespace vamp::binding "configuration"_a, "environment"_a = vamp::collision::Environment()); + MF("project_to_valid", + project_to_valid, + "Projects a configuration to multiple valid candidates using SDF gradient descent.", + "configuration"_a, + "environment"_a, + "steps"_a = 100, + "learning_rate"_a = 0.5f, + "noise_scale"_a = 0.1f); + MF("roadmap", PRM::roadmap, "PRM roadmap construction.", From 7a5c720d0286a6a3bc7f32ff709c7a1563076f6a Mon Sep 17 00:00:00 2001 From: Weihang Guo Date: Fri, 23 Jan 2026 18:03:25 -0600 Subject: [PATCH 10/10] Fix sdf --- scripts/evaluate_sdf.py | 7 +- scripts/sdf_descent.py | 174 +++++++++++++++++++++++++ src/impl/vamp/bindings/robot_helper.hh | 4 +- src/impl/vamp/optimization/sdf.hh | 16 +-- 4 files changed, 184 insertions(+), 17 deletions(-) create mode 100644 scripts/sdf_descent.py diff --git a/scripts/evaluate_sdf.py b/scripts/evaluate_sdf.py index 02f7708c..e31f5df7 100644 --- a/scripts/evaluate_sdf.py +++ b/scripts/evaluate_sdf.py @@ -96,9 +96,10 @@ def benchmark_solver(steps): # project_to_valid returns a list of valid configurations (or candidates) # C++ signature: returns std::vector candidates = robot.project_to_valid(q_init, env, steps=steps, learning_rate=0.5, noise_scale=0.1) - - if len(candidates) > 0: - valid_count += 1 + for q_cand in candidates: + if robot.sdf(q_cand, env) >= 0: + valid_count += 1 + break end_t = time.time() elapsed = end_t - start_t diff --git a/scripts/sdf_descent.py b/scripts/sdf_descent.py new file mode 100644 index 00000000..8ad661aa --- /dev/null +++ b/scripts/sdf_descent.py @@ -0,0 +1,174 @@ +import numpy as np +import vamp +import time +from fire import Fire + +def project_to_valid(robot, q, env, steps=100, learning_rate=0.5, noise_scale=0.1): + n_candidates = 8 + dim = len(q) + + # Initialize candidates with noise + # Use fixed seed for reproducibility matching C++ + rng = np.random.default_rng(42) + + # Broadcast and add noise + candidates = np.tile(q, (n_candidates, 1)) + noise = rng.uniform(-noise_scale, noise_scale, (n_candidates, dim)) + candidates += noise + + for step in range(steps): + # 1. Compute SDFs + dists = np.zeros(n_candidates) + for i in range(n_candidates): + dists[i] = robot.sdf(candidates[i], env) + + # 2. No early exit - we want to maximize SDF + + # 3. Compute Gradients (Finite Difference) + grads = np.zeros_like(candidates) + h = 1e-4 + + for i in range(n_candidates): + # Compute gradient for all, even if safe + original_q = candidates[i].copy() + for d in range(dim): + val = original_q[d] + + # f(x+h) + candidates[i, d] = val + h + f_plus = robot.sdf(candidates[i], env) + + # f(x-h) + candidates[i, d] = val - h + f_minus = robot.sdf(candidates[i], env) + + # Restore + candidates[i, d] = val + + grads[i, d] = (f_plus - f_minus) / (2 * h) + + # 4. Update Rule: Gradient Ascent + # q_new = q + lr * grad + for i in range(n_candidates): + candidates[i] += grads[i] * learning_rate + + return candidates + +def once(robot_name: str = "panda"): + # Load robot module + if not hasattr(vamp, robot_name): + print(f"Robot {robot_name} not found in vamp.") + return + + robot = getattr(vamp, robot_name) + + # Create environment + env = vamp.Environment() + obstacles = [ + ([0.5, 0.0, 0.5], 0.2), + ([0.0, 0.5, 0.5], 0.2), + ([0.5, 0.5, 0.5], 0.2), + ([0.3, -0.3, 0.3], 0.15), + ([-0.3, 0.3, 0.3], 0.15), + ([0.6, 0.0, 0.2], 0.1), + ] + for center, radius in obstacles: + env.add_sphere(vamp.Sphere(center, radius)) + + print(f"Benchmarking Python Solver for {robot_name}") + + rng = robot.halton() + rng.reset() + + q_init = rng.next() + + initial_valid = robot.sdf(q_init, env) >= 0 + initial_dist = robot.sdf(q_init, env) + + # Test 10 steps + # Lowered learning rate to 0.05 since we are doing direct gradient steps now + candidates_10 = project_to_valid(robot, q_init, env, steps=10, learning_rate=0.05, noise_scale=0.1) + dist_10 = np.max([robot.sdf(q, env) for q in candidates_10]) + candidates_100 = project_to_valid(robot, q_init, env, steps=100, learning_rate=0.05, noise_scale=0.1) + dist_100 = np.max([robot.sdf(q, env) for q in candidates_100]) + candidates_1000 = project_to_valid(robot, q_init, env, steps=1000, learning_rate=0.05, noise_scale=0.1) + dist_1000 = np.max([robot.sdf(q, env) for q in candidates_1000]) + print(f"Inital Dist: {initial_dist}, Dist 10: {dist_10}, Dist 100: {dist_100}, Dist 1000: {dist_1000}") + candidates_10000 = project_to_valid(robot, q_init, env, steps=10000, learning_rate=0.05, noise_scale=0.1) + dist_10000 = np.max([robot.sdf(q, env) for q in candidates_10000]) + print(f"Inital Dist: {initial_dist}, Dist 10: {dist_10}, Dist 100: {dist_100}, Dist 1000: {dist_1000}, Dist 10000: {dist_10000}") + return initial_valid, dist_10, dist_100, dist_1000, dist_10000 + +def bench(robot_name: str = "panda", n_samples: int = 100): + # Load robot module + if not hasattr(vamp, robot_name): + print(f"Robot {robot_name} not found in vamp.") + return + + robot = getattr(vamp, robot_name) + + # Create environment + env = vamp.Environment() + obstacles = [ + ([0.5, 0.0, 0.5], 0.2), + ([0.0, 0.5, 0.5], 0.2), + ([0.5, 0.5, 0.5], 0.2), + ([0.3, -0.3, 0.3], 0.15), + ([-0.3, 0.3, 0.3], 0.15), + ([0.6, 0.0, 0.2], 0.1), + ] + for center, radius in obstacles: + env.add_sphere(vamp.Sphere(center, radius)) + + print(f"Benchmarking Python Solver for {robot_name}") + + rng = robot.halton() + rng.reset() + + valid_count_10 = 0 + valid_count_100 = 0 + initial_valid_count = 0 + + start_time = time.time() + + print(f"Running {n_samples} queries...") + + for i in range(n_samples): + q_init = rng.next() + + initial_valid = robot.sdf(q_init, env) >= 0 + + if initial_valid: + initial_valid_count += 1 + + # Test 10 steps + candidates_10 = project_to_valid(robot, q_init, env, steps=10, learning_rate=0.05, noise_scale=0.1) + found_valid = False + for q in candidates_10: + if robot.sdf(q, env) >= 0: + found_valid = True + break + if found_valid: + valid_count_10 += 1 + + # Test 100 steps (independent run for stats, though inefficient) + candidates_100 = project_to_valid(robot, q_init, env, steps=100, learning_rate=0.05, noise_scale=0.1) + found_valid = False + for q in candidates_100: + if robot.sdf(q, env) >= 0: + found_valid = True + break + if found_valid: + valid_count_100 += 1 + + elapsed = time.time() - start_time + + print(f"Total Time: {elapsed:.2f}s") + print(f"Initial Valid: {initial_valid_count}/{n_samples} ({initial_valid_count/n_samples*100:.1f}%)") + print(f"Success Rate (10 steps): {valid_count_10}/{n_samples} ({valid_count_10/n_samples*100:.1f}%)") + print(f"Success Rate (100 steps): {valid_count_100}/{n_samples} ({valid_count_100/n_samples*100:.1f}%)") + + +if __name__ == "__main__": + bench() + diff --git a/src/impl/vamp/bindings/robot_helper.hh b/src/impl/vamp/bindings/robot_helper.hh index 42142ce3..a611b532 100644 --- a/src/impl/vamp/bindings/robot_helper.hh +++ b/src/impl/vamp/bindings/robot_helper.hh @@ -356,7 +356,7 @@ namespace vamp::binding const Type &c_in, const EnvironmentInput &environment, int steps = 10, - float learning_rate = 0.5f, + float learning_rate = 0.05f, float noise_scale = 0.1f) -> std::vector { auto result_block = vamp::optimization::project_to_valid( @@ -661,7 +661,7 @@ namespace vamp::binding "configuration"_a, "environment"_a, "steps"_a = 100, - "learning_rate"_a = 0.5f, + "learning_rate"_a = 0.05f, "noise_scale"_a = 0.1f); MF("roadmap", diff --git a/src/impl/vamp/optimization/sdf.hh b/src/impl/vamp/optimization/sdf.hh index 0d1a8fab..71db933e 100644 --- a/src/impl/vamp/optimization/sdf.hh +++ b/src/impl/vamp/optimization/sdf.hh @@ -51,7 +51,7 @@ namespace vamp::optimization const typename Robot::Configuration &start_state, const collision::Environment> &environment, int steps = 100, - float learning_rate = 0.5f, + float learning_rate = 0.05f, float noise_scale = 0.1f) noexcept -> typename Robot::template ConfigurationBlock { using ConfigBlock = typename Robot::template ConfigurationBlock; @@ -76,26 +76,18 @@ namespace vamp::optimization current_state[i] = Vector(noise_vals.data(), false); } - auto zero = Vector::fill(0.0f); auto lr = Vector::fill(learning_rate); for (int step = 0; step < steps; ++step) { - // Calculate SDF - auto dist = Robot::sdf(environment, current_state); - // Calculate Gradient auto grad = compute_gradient(environment, current_state); - // Update Rule - auto neg_dist = -dist; - auto relu_neg_dist = neg_dist.max(zero); - auto magnitude = lr * relu_neg_dist; - - // Apply update per dimension + // Update Rule: Gradient Ascent + // q_new = q + lr * grad for (std::size_t i = 0; i < Robot::dimension; ++i) { - auto delta = grad[i] * magnitude; + auto delta = grad[i] * lr; current_state[i] = current_state[i] + delta; } }