feat: Compute the signed distance field and project the configuration into a collision-free state.

CMakeLists.txt

@@ -141,10 +141,17 @@ endif()
 if(VAMP_BUILD_CPP_DEMO)
   add_executable(vamp_rrtc_example scripts/cpp/rrtc_example.cc)
   target_link_libraries(vamp_rrtc_example PRIVATE vamp_cpp)
-
-  # Disable strict warnings for demos to maintain compatibility
+
+  add_executable(vamp_sdf_example scripts/cpp/sdf_example.cc)
+  target_link_libraries(vamp_sdf_example PRIVATE vamp_cpp)
+
+  add_executable(vamp_sdf_example_manual scripts/cpp/sdf_example_manual.cc)
+  target_link_libraries(vamp_sdf_example_manual PRIVATE vamp_cpp)
+
   if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
     target_compile_options(vamp_rrtc_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
+    target_compile_options(vamp_sdf_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
+    target_compile_options(vamp_sdf_example_manual PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
   endif()
 endif()
 

scripts/cpp/sdf_example.cc

@@ -0,0 +1,101 @@
+#include <chrono>
+#include <vector>
+#include <array>
+#include <utility>
+#include <iostream>
+
+#include <vamp/collision/factory.hh>
+#include <vamp/robots/panda.hh>
+#include <vamp/random/halton.hh>
+#include <vamp/optimization/project.hh>
+
+using Robot = vamp::robots::Panda;
+static constexpr const std::size_t rake = vamp::FloatVectorWidth;
+using EnvironmentInput = vamp::collision::Environment<float>;
+using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;
+
+// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below
+static const std::vector<std::array<float, 3>> 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.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 main(int, char **) -> int
+{
+    // Build sphere cage 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);
+
+    // Benchmark
+    vamp::rng::Halton<Robot> sampler;
+    int n_samples = 1000;
+    int n_success = 0;
+    double total_time_ms = 0.0;
+    int total_iter = 0;
+    std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl;
+    int i = 0;
+    while (i < n_samples)
+    {
+        auto q_random = sampler.next();
+        // std::cout << "Sample " << i << ": " << q_random << std::endl;
+        Robot::ConfigurationBlock<rake> b;
+        for (auto k = 0U; k < Robot::dimension; ++k)
+        {
+            b[k] = q_random.broadcast(k);
+        }
+        auto valid = Robot::fkcc<rake>(env_v, b);
+        if (valid)
+        {
+            continue;
+        }
+        i++;
+
+        // Project
+        auto start_t = std::chrono::steady_clock::now();
+
+        auto b_final = vamp::optimization::project_to_valid<Robot, rake>(b, env_v);
+
+        auto end_t = std::chrono::steady_clock::now();
+        auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(end_t - start_t);
+        valid = Robot::fkcc<rake>(env_v, b_final);
+        if (valid)
+        {
+            n_success++;
+        }
+        total_time_ms += dur.count() / 1e6;
+    }
+
+    std::cout << "Benchmark Results:" << std::endl;
+    std::cout << "Total Samples: " << n_samples << std::endl;
+    std::cout << "Successful Projections: " << n_success << " ("
+              << (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl;
+    if (n_success > 0)
+    {
+        std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl;
+    }
+
+    return 0;
+}

scripts/cpp/sdf_example_manual.cc

@@ -0,0 +1,154 @@
+#include <chrono>
+#include <vector>
+#include <array>
+#include <utility>
+#include <iostream>
+
+#include <vamp/collision/factory.hh>
+#include <vamp/robots/panda.hh>
+#include <vamp/random/halton.hh>
+
+using Robot = vamp::robots::Panda;
+static constexpr const std::size_t rake = vamp::FloatVectorWidth;
+using EnvironmentInput = vamp::collision::Environment<float>;
+using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;
+
+// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below
+static const std::vector<std::array<float, 3>> 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.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 main(int, char **) -> int
+{
+    // Build sphere cage 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);
+
+    // Benchmark
+    vamp::rng::Halton<Robot> sampler;
+    int n_samples = 1000;
+    int n_success = 0;
+    double total_time_ms = 0.0;
+    int total_iter = 0;
+    std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl;
+    int i = 0;
+    while (i < n_samples)
+    {
+        auto q_random = sampler.next();
+        // std::cout << "Sample " << i << ": " << q_random << std::endl;
+        Robot::ConfigurationBlock<rake> b;
+        for (auto k = 0U; k < Robot::dimension; ++k)
+        {
+            b[k] = q_random.broadcast(k);
+        }
+        auto valid = Robot::fkcc<rake>(env_v, b);
+        if (valid)
+        {
+            continue;
+        }
+        i++;
+
+        // Project
+        auto start_t = std::chrono::steady_clock::now();
+
+        float current_min_dist = -1e9f;
+        int iter = 0;
+        const int max_iters = 100;
+        float alpha = 0.1f;
+
+        while (current_min_dist < 0 && iter < max_iters)
+        {
+            // Re-evaluate
+            auto res = Robot::sdf_gradient(env_v, b);
+
+            auto dists_arr = res.first.to_array();
+            current_min_dist = 1e9f;
+            for (auto d : dists_arr)
+            {
+                if (d < current_min_dist)
+                {
+                    current_min_dist = d;
+                }
+            }
+
+            if (current_min_dist >= 0)
+            {
+                break;
+            }
+
+            Robot::ConfigurationBlock<rake> dq_block;
+            Robot::d_collision_d_q(b, res.second, dq_block);  // b is already the block for q_new
+            std::vector<float> dq(Robot::dimension);
+            for (auto k = 0U; k < Robot::dimension; ++k)
+            {
+                dq[k] = dq_block[k].element(0);
+            }
+
+            float dq_norm = 0.0f;
+            for (float v : dq)
+            {
+                dq_norm += v * v;
+            }
+            dq_norm = std::sqrt(dq_norm);
+
+            if (dq_norm > std::numeric_limits<float>::epsilon())
+            {
+                for (auto k = 0U; k < Robot::dimension; ++k)
+                {
+                    b[k] = b[k] + alpha * (dq[k] / dq_norm);
+                }
+            }
+            else
+            {
+                break;
+            }
+            iter++;
+        }
+        auto end_t = std::chrono::steady_clock::now();
+        auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(end_t - start_t);
+        auto b_final = b;
+        valid = Robot::fkcc<rake>(env_v, b_final);
+        if (valid)
+        {
+            n_success++;
+        }
+        total_time_ms += dur.count() / 1e6;
+        total_iter += iter;
+    }
+
+    std::cout << "Benchmark Results:" << std::endl;
+    std::cout << "Total Samples: " << n_samples << std::endl;
+    std::cout << "Successful Projections: " << n_success << " ("
+              << (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl;
+    if (n_success > 0)
+    {
+        std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl;
+        std::cout << "Average Iterations: " << (total_iter / n_samples) << std::endl;
+    }
+
+    return 0;
+}

scripts/sdf_example.py

@@ -0,0 +1,78 @@
+import numpy as np
+from viser_utils import setup_viser_with_robot, add_new_robot_to_server, add_spheres
+from pathlib import Path
+
+import vamp
+from fire import Fire
+
+# Starting configuration
+a = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+
+# Goal configuration
+b = [2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785]
+
+# Problem specification: a list of sphere centers
+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],
+    ]
+
+
+def main(
+    obstacle_radius: float = 0.2,
+    attachment_radius: float = 0.07,
+    attachment_offset: float = 0.02,
+    planner: str = "rrtc",
+    **kwargs,
+    ):
+
+    (vamp_module, planner_func, plan_settings,
+     simp_settings) = (vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs))
+
+    robot_dir = Path(__file__).parents[1] / "resources" / "panda"
+    server, robot_original = setup_viser_with_robot(robot_dir, "panda_spherized.urdf")
+    robot_projected = add_new_robot_to_server(
+        server,
+        robot_dir,
+        "panda_spherized.urdf",
+        root_node_name = "/robot_projected",
+        mesh_color_override = (0, 255, 0, 0.5)
+        )
+
+    e = vamp.Environment()
+    for sphere in problem:
+        e.add_sphere(vamp.Sphere(sphere, obstacle_radius))
+
+    _problem_sphere_handles = add_spheres(
+        server, np.array(problem), np.array([obstacle_radius] * len(problem))
+        )
+
+    sampler = vamp_module.halton()
+    # Add button to sample and project
+    sample_button = server.gui.add_button("Sample and Project")
+
+    @sample_button.on_click
+    def _(event):
+        q = sampler.next()
+        q_projected = vamp.panda.project_to_valid(q, e)
+        robot_original.update_cfg(q)
+        robot_projected.update_cfg(q_projected)
+
+    while True:
+        continue
+
+
+if __name__ == "__main__":
+    Fire(main)

scripts/viser_utils.py

@@ -20,6 +20,17 @@ def setup_viser_with_robot(robot_dir, robot_urdf_name):
     return server, robot
 
 
+def add_new_robot_to_server(server, robot_dir, robot_urdf_name, **kwargs):
+    urdf = yourdfpy.URDF.load(str(robot_dir / robot_urdf_name))
+    robot = ViserUrdf(
+        server,
+        urdf,
+        load_meshes = True,
+        load_collision_meshes = False,
+        **kwargs,
+        )
+    return robot
+
 def add_point_cloud(
     server: viser.ViserServer,
     point_cloud: np.ndarray,