Add MAB-RRT planner with adaptive cylinder sampling

- MAB-RRT: Multi-Armed Bandit RRT with PCA-guided cylindrical sampling
- Adaptive burn-in phase discovers valid motion directions
- 3-arm MAB: UNIFORM, CYLINDER_UP, CYLINDER_DOWN
- CylinderSampler with 2D/3D PCA support
- Demo with 6D assembly and 2D bug trap scenarios
- Requires adaptive_max_radius in YAML config (no hardcoded fallbacks)

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude Opus 4.5 <noreply@anthropic.com>

Author: serboba

demos/CMakeLists.txt

@@ -62,6 +62,10 @@ if (OMPL_BUILD_DEMOS)
     add_ompl_demo(demo_MAB_RRT disassembly/MAB_RRT_Demo.cpp)
     target_link_libraries(demo_MAB_RRT PRIVATE yaml-cpp::yaml-cpp)
     
+    # MAB-RRT Benchmark - Compare planners on occupancy grid
+    add_ompl_demo(demo_benchmark_occupancy_grid disassembly/benchmark_occupancy_grid.cpp)
+    target_link_libraries(demo_benchmark_occupancy_grid PRIVATE yaml-cpp::yaml-cpp)
+    
     # Copy config file to build directory
     configure_file(
         ${CMAKE_CURRENT_SOURCE_DIR}/disassembly/benchmark_baseline.yaml

demos/disassembly/MAB_RRT_Demo.cpp

@@ -1,13 +1,13 @@
-### MAB-SSRRT Configuration
-# Configuration file for MAB-SSRRT (Multi-Armed Bandit Sphere-Sampled RRT) planner
+### MAB-RRT Configuration
+# Configuration file for MAB-RRT (Multi-Armed Bandit RRT) planner
 
 # Goal Bias
-uniform_goal_bias : 0.75
+uniform_goal_bias : 0.1
 sphere_goal_bias : 0.0000001
 
 # MAB Parameters
 #
-# MAB-SSRRT uses a flat 3-arm MAB:
+# MAB-RRT uses a flat 3-arm MAB:
 #   - Arm 0: UNIFORM sampling
 #   - Arm 1: CYLINDER_UP sampling (along +Z axis)
 #   - Arm 2: CYLINDER_DOWN sampling (along -Z axis)
@@ -16,53 +16,65 @@ mab_uniform_sphere_window_size : 256    # Sliding window size for MAB arm select
 
 # If > 0, after this many consecutive valid CYLINDER samples, the planner forces one UNIFORM sample on the next iteration.
 # 0 = feature disabled (default, preserves current behavior)
-forced_uniform_after_cylinder_valid_streak: 6
+forced_uniform_after_cylinder_valid_streak: 5
 
 # Adaptive Sphere Sampling Parameters
-adaptive_quasirandom_sample_size : 8
+# Adaptive Sphere Sampling Parameters
+adaptive_quasirandom_sample_size : 64
 adaptive_start_radius : 1.0
-adaptive_min_radius : 0.00001
-adaptive_shrink_step : 0.9
-adaptive_grow_step : 0.1
+adaptive_min_radius : 0.000001
+adaptive_max_radius : 25.0
+# Direct multipliers (no exp() applied)
+# To maintain same behavior as exp(-0.7) and exp(0.9):
+#   exp(-0.7) ≈ 0.496585 (shrinks by this factor)
+#   exp(0.9) ≈ 2.459603 (grows by this factor)
+adaptive_shrink_step : 0.496585
+adaptive_grow_step : 2.459603
 adaptive_min_expected_validity_rate : 0.10
 adaptive_max_expected_validity_rate: 0.50
-adaptive_burnin_max_steps: 10
+adaptive_burnin_max_steps: 50
 
 pca_filter_top_percent: 1.0  # Percentage of top PCA components to keep, default 1.0 to use all valid samples for online PCA recalibration
 
 # Initial Uniform Sampling Check
 # Before starting adaptive sphere burn-in, try k uniform samples.
 # If validity rate > probability, skip adaptive sphere and use uniform only.
 initialFreeSamplingProbability: 0.20
-initialNumberOfUniformSampleTrials: 10
+initialNumberOfUniformSampleTrials: 0
 
 # Cylinder Sampling Configuration
 enableDynamicCylinderPCA: true # Enable dynamic recomputation of cylinder PCA axis
 
 # Burn-in Early Exit
-# If true, MAB-SSRRT will abort planning after burn-in when not enough valid samples were found around the origin.
-burnin_early_exit_enabled: true
+# If true, MAB-RRT will abort planning after burn-in when not enough valid samples were found around the origin.
+burnin_early_exit_enabled: false
 # Minimum number of valid burn-in samples required to continue planning.
 # If total_valid_samples < this value AND burnin_early_exit_enabled == true, the planner will exit early from solve().
-burnin_min_valid_samples_for_continue: 3
+burnin_min_valid_samples_for_continue: 2
 
 # If true, burn-in will exit early if validity rate is 1.0 for consecutiveFullValidityMaxIterations steps
-earlyExitOnConsecutiveFullValidity: true
+earlyExitOnConsecutiveFullValidity: false
 consecutiveFullValidityMaxIterations: 5
 
 # Sphere sampler values
 
-sphere_extension_eps : 0.3 # if sphere sample was walid std::exp(sphere_extension_eps)*currentBestRadius
-                            # to grow the cylinder and sphere as fas as possible 
-                            # also used for kappa maybe add another param for kappa
+# Extension factor for cylinder height extension (EXPONENTIAL GROWTH)
+# extensionHeight = bestRadius * (exp(sphere_extension_eps - 1.0) - 1.0)
+# This allows fast exponential growth even when bestRadius is very small (e.g., 0.001)
+# Examples:
+#   sphere_extension_eps = 1.0 → extensionHeight = 0 (no extension)
+#   sphere_extension_eps = 2.0 → extensionHeight = bestRadius * (exp(1.0) - 1.0) ≈ bestRadius * 1.718
+#   sphere_extension_eps = 3.0 → extensionHeight = bestRadius * (exp(2.0) - 1.0) ≈ bestRadius * 6.389
+#   sphere_extension_eps = 4.0 → extensionHeight = bestRadius * (exp(3.0) - 1.0) ≈ bestRadius * 19.085
+sphere_extension_eps : 2.0 # 1.0 = no extension, >1.0 = exponential growth via exp(value - 1.0)
 
 # MAB Reward System
 # Fixed reward parameters for MAB arm selection
 uniform_sampler_fixed_valid_reward: 9999999.0  # Fixed reward for valid uniform samples (high to encourage exploration when disassembled)
 uniform_sampler_invalid_reward: 0.0         # Fixed reward for invalid uniform samples
 
 sphere_sampler_fixed_valid_reward: 5.0      # Fixed reward for valid cylinder samples
-sphere_sampler_invalid_reward: 1.00         # Fixed reward for invalid cylinder samples (small positive to discourage uniform when assembled)
+sphere_sampler_invalid_reward: 0.00         # Fixed reward for invalid cylinder samples (small positive to discourage uniform when assembled)
 
 # Cylinder Sampling Parameters
 cylinder_radius_offset_multiplier: 0.1     # Offset multiplier for cylinder radius in PCA fitting

demos/disassembly/benchmark_baseline.yaml

@@ -74,6 +74,8 @@ namespace ompl
             void setCylinderRadiusOffsetMultiplier(double val) { cylinderSampler_.setRadiusOffsetMultiplier(val); }
             void setFibonacciJitterRadius(double val) { fibSampler_.setJitterRadius(val); }
             void setPCAFilterTopPercent(double val) { pcaFilterTopPercent_ = val; }
+            void freezeCylinder() { cylinderFrozen_ = true; }  // Prevent cylinder refitting during planning
+            void setCylinderHeight(double height) { cylinderSampler_.setCylinderHeight(height); }
 
             void updateComponentCenter(int componentIndex);
             void appendFibonacciSamples(double customRadius = -1.0);
@@ -108,6 +110,7 @@ namespace ompl
             bool validPointsDirty_ = true;
             Eigen::Vector3d cylinderAxis_ = Eigen::Vector3d::UnitZ();
             bool hasCylinderAxis_ = false;
+            bool cylinderFrozen_ = false;  // If true, don't refit cylinder during planning
 
             /** @brief Project 3D sphere point to 2D circle (in-place modification) */
             void projectSphereToCircle(Point& p, double radius);

src/ompl/base/samplers/sphere/AdaptiveSphereSampler.h

@@ -51,6 +51,7 @@ namespace ompl
             // Accessors
             const Cylinder& getCylinder() const { return cylinder_; }
             bool hasValidCylinder() const { return hasValidCylinder_; }
+            void setCylinderHeight(double height) { cylinder_.height = height; }
 
         private:
             Cylinder cylinder_;

src/ompl/base/samplers/sphere/CylinderSampler.h

@@ -187,15 +187,42 @@ double AdaptiveSphereSampler::getValidSampleRate() const
 
 AdaptiveSphereSampler::Point AdaptiveSphereSampler::getRandomSampleFromCylinder(double extensionHeight, int chosenDirection)
 {
-    // Use cached cylinder if available and valid
-    if (hasCylinderAxis_)
-    {
-         cylinderSampler_.fitWithAxis(allValidPoints_, cylinderAxis_, cylinderSampler_.getRadiusOffsetMultiplier());
-    }
-    else if (validPointsDirty_)
+    // DEBUG: Print cylinder state before sampling
+    const auto& cyl = cylinderSampler_.getCylinder();
+    OMPL_INFORM("DEBUG ADAPTIVE_SPHERE: getRandomSampleFromCylinder called with extensionHeight=%.6f, direction=%d", 
+               extensionHeight, chosenDirection);
+    OMPL_INFORM("DEBUG ADAPTIVE_SPHERE: Current cylinder height=%.6f, radius=%.6f, hasValidCylinder=%d", 
+               cyl.height, cyl.radius, cylinderSampler_.hasValidCylinder());
+    OMPL_INFORM("DEBUG ADAPTIVE_SPHERE: All valid points count=%zu", allValidPoints_.size());
+    
+    // If cylinder is frozen (after burn-in), don't refit - use existing cylinder
+    if (!cylinderFrozen_)
     {
-        cylinderSampler_.fit(allValidPoints_, pcaFilterTopPercent_);
-        validPointsDirty_ = false;
+        // Preserve the height before refitting (it was set to bestRadius during burn-in)
+        double preservedHeight = cylinderSampler_.getCylinder().height;
+        
+        // Use cached cylinder if available and valid
+        if (hasCylinderAxis_)
+        {
+             cylinderSampler_.fitWithAxis(allValidPoints_, cylinderAxis_, cylinderSampler_.getRadiusOffsetMultiplier());
+             // Restore the preserved height (only update axis, not height)
+             if (preservedHeight > 0.0) {
+                 cylinderSampler_.setCylinderHeight(preservedHeight);
+             }
+             OMPL_INFORM("DEBUG ADAPTIVE_SPHERE: Refitted cylinder with axis, preserved height=%.6f", 
+                        preservedHeight);
+        }
+        else if (validPointsDirty_)
+        {
+            cylinderSampler_.fit(allValidPoints_, pcaFilterTopPercent_);
+            validPointsDirty_ = false;
+            // Restore the preserved height (only update axis, not height)
+            if (preservedHeight > 0.0) {
+                cylinderSampler_.setCylinderHeight(preservedHeight);
+            }
+            OMPL_INFORM("DEBUG ADAPTIVE_SPHERE: Refitted cylinder, preserved height=%.6f", 
+                       preservedHeight);
+        }
     }
 
     Point sample = cylinderSampler_.sampleCylinder(extensionHeight, chosenDirection, cylinderSampler_.getSamplingRadiusMultiplier(), rng_);
@@ -210,7 +237,16 @@ AdaptiveSphereSampler::Point AdaptiveSphereSampler::getRandomSampleFromCylinder(
 
 AdaptiveSphereSampler::Point AdaptiveSphereSampler::getRandomSampleFromCylinder(double extensionHeight, int chosenDirection, const Eigen::Vector3d &axis)
 {
+    // Preserve the height before refitting (it was set to bestRadius during burn-in)
+    double preservedHeight = cylinderSampler_.getCylinder().height;
+    
     cylinderSampler_.fitWithAxis(allValidPoints_, axis, cylinderSampler_.getRadiusOffsetMultiplier());
+    
+    // Restore the preserved height (only update axis, not height)
+    if (preservedHeight > 0.0) {
+        cylinderSampler_.setCylinderHeight(preservedHeight);
+    }
+    
     Point sample = cylinderSampler_.sampleCylinder(extensionHeight, chosenDirection, cylinderSampler_.getSamplingRadiusMultiplier(), rng_);
 
     // Project to 2D circle if needed

src/ompl/base/samplers/sphere/src/AdaptiveSphereSampler.cpp

@@ -9,6 +9,7 @@
 
 #include <ompl/base/samplers/sphere/CylinderSampler.h>
 #include <ompl/datastructures/geometry/GeometryUtils.h>
+#include <ompl/util/Console.h>
 #include <algorithm>
 #include <cmath>
 #include <limits>
@@ -154,6 +155,15 @@ void CylinderSampler::fitWithAxis(const std::vector<Point>& points, const Eigen:
 CylinderSampler::Point CylinderSampler::sampleCylinder(double extensionHeight, int direction, double radiusMultiplier, std::mt19937& rng)
 {
     if (!hasValidCylinder_) throw std::runtime_error("Cylinder not initialized");
+    
+    // DEBUG: Print cylinder sampling parameters
+    OMPL_INFORM("DEBUG CYLINDER_SAMPLER: sampleCylinder called with extensionHeight=%.6f, direction=%d, radiusMultiplier=%.6f", 
+               extensionHeight, direction, radiusMultiplier);
+    OMPL_INFORM("DEBUG CYLINDER_SAMPLER: Current cylinder height=%.6f, radius=%.6f", 
+               cylinder_.height, cylinder_.radius);
+    OMPL_INFORM("DEBUG CYLINDER_SAMPLER: Will sample from height %.6f to %.6f (extension adds %.6f)", 
+               cylinder_.height, cylinder_.height + extensionHeight, extensionHeight);
+    
     return samplePointInternal(cylinder_, extensionHeight, direction, cylinder_.radius * radiusMultiplier, rng);
 }
 
@@ -177,34 +187,30 @@ CylinderSampler::Point CylinderSampler::samplePointInternal(const Cylinder& cyl,
     bool is2D = (std::fabs(cyl.az) < 1e-6);
 
     if (is2D) {
-        // 2D case: Cylinder becomes a line
-        // Sample along the line: h * axis + perpendicular jitter
-        // For 2D, perpendicular to axis (ax, ay) is: (-ay, ax) or (ay, -ax)
-        // Normalize the axis first
+        // 2D case: Cylinder becomes a line segment
+        // Sample only along the axis, NO perpendicular jitter
+        // In 2D, adding perpendicular jitter would make total distance = sqrt(h² + r²),
+        // which is incorrect. We want samples to lie exactly on the line.
         double axis_norm = std::sqrt(cyl.ax * cyl.ax + cyl.ay * cyl.ay);
         if (axis_norm < 1e-10) {
-            // Degenerate axis, use default
-            double px = r * std::cos(theta);
-            double py = r * std::sin(theta);
+            // Degenerate axis, sample at origin
+            double px = 0.0;
+            double py = 0.0;
             double pz = 0.0;
-            double newRadius_ = std::sqrt(px * px + py * py);
+            double newRadius_ = 0.0;
             return Point{px, py, pz, newRadius_, true};
         }
 
         double ax_norm = cyl.ax / axis_norm;
         double ay_norm = cyl.ay / axis_norm;
 
-        // Perpendicular vector: (-ay, ax) - this is 90 degree rotation
-        double perp_x = -ay_norm;
-        double perp_y = ax_norm;
-        
-        // Sample: h * axis + r * (cos(θ) * perp_x + sin(θ) * perp_y)
-        // For 2D line, we sample along axis with perpendicular jitter
-        double px = h * ax_norm + r * (std::cos(theta) * perp_x + std::sin(theta) * perp_y);
-        double py = h * ay_norm + r * (std::cos(theta) * perp_y - std::sin(theta) * perp_x);
+        // Sample only along the axis: h * axis (no perpendicular jitter)
+        double px = h * ax_norm;
+        double py = h * ay_norm;
         double pz = 0.0;  // Always 0 for 2D
 
-        double newRadius_ = std::sqrt(px * px + py * py);
+        // Distance from origin is just |h| (absolute value of distance along axis)
+        double newRadius_ = std::abs(h);
 
         return Point{px, py, pz, newRadius_, true};
     }