Feature: Signed Distance Field

CMakeLists.txt

@@ -146,6 +146,13 @@ 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()
 endif()
 
 # OMPL integration demo

scripts/cpp/sdf_example.cc

@@ -0,0 +1,185 @@
+#include <vector>
+#include <array>
+#include <utility>
+#include <iostream>
+#include <iomanip>
+#include <chrono>
+
+#include <vamp/collision/factory.hh>
+#include <vamp/robots/panda.hh>
+#include <vamp/optimization/sdf.hh>
+#include <vamp/random/halton.hh>
+
+// 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<float>;
+using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;
+
+// Environment Setup from rrtc_example.cc
+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.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 <typename Func>
+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<double, std::milli> 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<Robot::Configuration> configs;
+    auto rng = std::make_shared<vamp::rng::Halton<Robot>>();
+
+    // Discard first few samples (Halton)
+    for(int i=0; i<100; ++i) rng->next();
+
+    for(int i=0; i<N_SAMPLES; ++i) {
+        // rng->next() 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<rake> block;
+        auto& cfg = configs[idx];
+        for (size_t d = 0; d < Robot::dimension; ++d) {
+             std::array<float, rake> row;
+             row.fill(cfg.element(d));
+             block[d] = Robot::ConfigurationBlock<rake>::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<Robot, rake>(
+            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<Robot, rake>(
+            configs[idx], 
+            env_v, 
+            100,    // steps
+            0.5f,  // learning rate
+            0.05f  // noise
+        );
+        volatile float val = valid_block[0].to_array()[0];
+        (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<rake> 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<float, rake> row;
+                     row.fill(cfg.element(d));
+                     block[d] = Robot::ConfigurationBlock<rake>::RowT(row.data(), false);
+                 }
+            } else {
+                block = vamp::optimization::project_to_valid<Robot, rake>(
+                    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;
+}

scripts/evaluate_sdf.py

@@ -0,0 +1,116 @@
+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<Type>
+             candidates = robot.project_to_valid(q_init, env, steps=steps, learning_rate=0.5, noise_scale=0.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
+        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)