diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1217634c..d2d4fb6e 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/scripts/cpp/sdf_example.cc b/scripts/cpp/sdf_example.cc
new file mode 100644
index 00000000..e5c9c2e8
--- /dev/null
+++ b/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;
+}
diff --git a/scripts/evaluate_sdf.py b/scripts/evaluate_sdf.py
new file mode 100644
index 00000000..e31f5df7
--- /dev/null
+++ b/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)
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 8fa3d7c1..a611b532 100644
--- a/src/impl/vamp/bindings/robot_helper.hh
+++ b/src/impl/vamp/bindings/robot_helper.hh
@@ -23,6 +23,7 @@
 #include <vamp/planning/rrtc.hh>
 #include <vamp/planning/aorrtc.hh>
 #include <vamp/vector.hh>
+#include <vamp/optimization/sdf.hh>
 
 #include <nanobind/nanobind.h>
 #include <nanobind/make_iterator.h>
@@ -266,6 +267,29 @@ 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 and in_bounds_out)) and
+                   vamp::planning::validate_motion<Robot, rake, 1>(
+                       configuration_in, configuration_out, EnvironmentVector(environment));
+        }
+
         inline static auto simplify(
             const Path &path,
             const EnvironmentInput &environment,
@@ -281,6 +305,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<rake>(EnvironmentVector(environment), Input::template block<rake>(c_in))
+                .to_array()[0];
+        }
+
         inline static auto filter_self_from_pointcloud(
             const std::vector<collision::Point> &pc,
             float point_radius,
@@ -319,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.05f,
+            float noise_scale = 0.1f) -> std::vector<Type>
+        {
+            auto result_block = vamp::optimization::project_to_valid<Robot, rake>(
+                Input::to(c_in),
+                EnvironmentVector(environment),
+                steps,
+                learning_rate,
+                noise_scale);
+
+            std::vector<Type> 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;
         }
     };
 
@@ -357,6 +418,26 @@ namespace vamp::binding
             "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<float, Robot::dimension> 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<float, Robot::dimension> 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<Robot>;
         nb::class_<typename RNG::Ptr>(submodule, "RNG", "RNG for robot configurations.")
             .def(
@@ -552,6 +633,20 @@ namespace vamp::binding
            "environment"_a = vamp::collision::Environment<float>(),
            "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<float>(),
+           "check_bounds"_a = false);
+
+        MF("sdf",
+           sdf,
+           "Compute the signed distance field (SDF) for a configuration.",
+           "configuration"_a,
+           "environment"_a = vamp::collision::Environment<float>());
+
         MF("filter_self_from_pointcloud",
            filter_self_from_pointcloud,
            "Removes points from pointcloud which collide with the robot and environment.",
@@ -560,6 +655,15 @@ namespace vamp::binding
            "configuration"_a,
            "environment"_a = vamp::collision::Environment<float>());
 
+        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.05f,
+           "noise_scale"_a = 0.1f);
+
         MF("roadmap",
            PRM::roadmap,
            "PRM roadmap construction.",
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 <typename DataT>
+    inline constexpr auto sphere_capsule_l2(
+        const Capsule<DataT> &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 <typename DataT>
+    inline constexpr auto sphere_capsule_l2(const Capsule<DataT> &c, const Sphere<DataT> &s) noexcept -> DataT
+    {
+        return sphere_capsule_l2(c, s.x, s.y, s.z, s.r);
+    }
+
+    template <typename DataT>
+    inline constexpr auto sphere_z_aligned_capsule_l2(
+        const Capsule<DataT> &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 <typename DataT>
+    inline constexpr auto sphere_z_aligned_capsule_l2(const Capsule<DataT> &c, const Sphere<DataT> &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 <typename DataT>
+    inline constexpr auto sphere_cuboid_l2(
+        const Cuboid<DataT> &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 <typename DataT>
+    inline constexpr auto sphere_cuboid_l2(const Cuboid<DataT> &c, const Sphere<DataT> &s) noexcept -> DataT
+    {
+        return sphere_cuboid_l2(c, s.x, s.y, s.z, s.r);
+    }
+
+    template <typename DataT>
+    inline constexpr auto sphere_z_aligned_cuboid_l2(
+        const Cuboid<DataT> &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 <typename DataT>
+    inline constexpr auto sphere_z_aligned_cuboid_l2(const Cuboid<DataT> &c, const Sphere<DataT> &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 <typename DataT>
+    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 <typename DataT>
+    inline constexpr auto sphere_sphere_l2(
+        const Sphere<DataT> &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 <typename DataT>
     inline constexpr auto sphere_sphere_l2(const Sphere<DataT> &a, const Sphere<DataT> &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 <typename DataT, typename ArgT1, typename ArgT2, typename ArgT3, typename ArgT4>
+    inline auto sphere_environment_sdf(
+        const collision::Environment<DataT> &e,
+        ArgT1 sx_,
+        ArgT2 sy_,
+        ArgT3 sz_,
+        ArgT4 sr_) noexcept -> DataT
+    {
+        auto sx = static_cast<DataT>(sx_);
+        auto sy = static_cast<DataT>(sy_);
+        auto sz = static_cast<DataT>(sz_);
+        auto sr = static_cast<DataT>(sr_);
+
+        auto min_dist = static_cast<DataT>(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 <typename DataT, typename ArgT1, typename ArgT2, typename ArgT3, typename ArgT4>
     inline auto sphere_environment_get_collisions(
         const collision::Environment<DataT> &e,  //
diff --git a/src/impl/vamp/optimization/sdf.hh b/src/impl/vamp/optimization/sdf.hh
new file mode 100644
index 00000000..71db933e
--- /dev/null
+++ b/src/impl/vamp/optimization/sdf.hh
@@ -0,0 +1,97 @@
+#pragma once
+
+#include <vamp/vector.hh>
+#include <vamp/collision/environment.hh>
+#include <random>
+#include <cmath>
+#include <iostream>
+
+namespace vamp::optimization
+{
+    // Computes Gradient of SDF via Central Difference
+    template <typename Robot, std::size_t rake>
+    inline auto compute_gradient(
+        const collision::Environment<FloatVector<rake>> &environment,
+        const typename Robot::template ConfigurationBlock<rake> &state,
+        float h = 1e-4f) noexcept -> typename Robot::template ConfigurationBlock<rake>
+    {
+        using ConfigBlock = typename Robot::template ConfigurationBlock<rake>;
+        
+        ConfigBlock grad;
+        auto h_vec = FloatVector<rake>::fill(h);
+        auto inv_2h = FloatVector<rake>::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 <typename Robot, std::size_t rake = FloatVectorWidth>
+    inline auto project_to_valid(
+        const typename Robot::Configuration &start_state,
+        const collision::Environment<FloatVector<rake>> &environment,
+        int steps = 100,
+        float learning_rate = 0.05f,
+        float noise_scale = 0.1f) noexcept -> typename Robot::template ConfigurationBlock<rake>
+    {
+        using ConfigBlock = typename Robot::template ConfigurationBlock<rake>;
+        using Vector = FloatVector<rake>;
+
+        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<float> dist_noise(-noise_scale, noise_scale);
+
+        for (std::size_t i = 0; i < Robot::dimension; ++i)
+        {
+            // Create an array for the row
+            std::array<float, rake> 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 lr = Vector::fill(learning_rate);
+
+        for (int step = 0; step < steps; ++step)
+        {
+            // Calculate Gradient
+            auto grad = compute_gradient<Robot, rake>(environment, current_state);
+
+            // Update Rule: Gradient Ascent
+            // q_new = q + lr * grad
+            for (std::size_t i = 0; i < Robot::dimension; ++i)
+            {
+                auto delta = grad[i] * lr;
+                current_state[i] = current_state[i] + delta;
+            }
+        }
+
+        return current_state;
+    }
+}
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 <std::size_t rake>
+        inline static auto sdf(
+            const vamp::collision::Environment<FloatVector<rake>> &environment,
+            const ConfigurationBlock<rake> &x) noexcept
+        {
+            Spheres<rake> spheres;
+            sphere_fk(x, spheres);
+
+            auto min_dist = FloatVector<rake>::fill(1.0e30);
+
+            auto update_min = [&](const FloatVector<rake> &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<std::vector<std::string>>, std::vector<std::pair<std::size_t, std::size_t>>>;
 
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 <std::size_t rake>
+        inline static auto sdf(
+            const vamp::collision::Environment<FloatVector<rake>> &environment,
+            const ConfigurationBlock<rake> &x) noexcept
+        {
+            Spheres<rake> spheres;
+            sphere_fk(x, spheres);
+
+            auto min_dist = FloatVector<rake>::fill(1.0e30);
+
+            auto update_min = [&](const FloatVector<rake> &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<std::vector<std::string>>, std::vector<std::pair<std::size_t, std::size_t>>>;
 
diff --git a/src/impl/vamp/robots/panda.hh b/src/impl/vamp/robots/panda.hh
index 0c51e3f9..7b7acadb 100644
--- a/src/impl/vamp/robots/panda.hh
+++ b/src/impl/vamp/robots/panda.hh
@@ -461,6 +461,31 @@ namespace vamp::robots
             }
         }
 
+        template <std::size_t rake>
+        inline static auto sdf(
+            const vamp::collision::Environment<FloatVector<rake>> &environment,
+            const ConfigurationBlock<rake> &x) noexcept -> FloatVector<rake>
+        {
+            Spheres<rake> spheres;
+            sphere_fk(x, spheres);
+
+            auto min_dist = FloatVector<rake>::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<std::vector<std::string>>, std::vector<std::pair<std::size_t, std::size_t>>>;
 
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 <std::size_t rake>
+        static auto sdf(
+            const vamp::collision::Environment<FloatVector<rake>> &environment,
+            const ConfigurationBlock<rake> &q) noexcept
+        {
+            return sphere_environment_sdf(environment, q[0], q[1], q[2], radius);
+        }
+
         using Debug = std::
             pair<std::vector<std::vector<std::string>>, std::vector<std::pair<std::size_t, std::size_t>>>;
 
diff --git a/src/impl/vamp/robots/ur5.hh b/src/impl/vamp/robots/ur5.hh
index 5cc815c7..8490ad49 100644
--- a/src/impl/vamp/robots/ur5.hh
+++ b/src/impl/vamp/robots/ur5.hh
@@ -397,6 +397,31 @@ namespace vamp::robots
             }
         }
 
+        template <std::size_t rake>
+        inline static auto sdf(
+            const vamp::collision::Environment<FloatVector<rake>> &environment,
+            const ConfigurationBlock<rake> &x) noexcept -> FloatVector<rake>
+        {
+            Spheres<rake> spheres;
+            sphere_fk(x, spheres);
+
+            auto min_dist = FloatVector<rake>::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<std::vector<std::string>>, std::vector<std::pair<std::size_t, std::size_t>>>;