Add adaptive_max_radius YAML parameter to clamp burn-in radius growth

- Add adaptive_max_radius parameter to YAML config
- Read adaptive_max_radius from YAML in both MAB_RRT and MAB_RRT_DEBUG
- Clamp radius to adaptive_max_radius during grow steps if it exceeds the limit
- If YAML value is set, it overrides computed state space bounds value
- Tested successfully with toy5.csv - radius clamped from 36.6 to 25.0

Author: serboba

demos/disassembly/benchmark_baseline.yaml

@@ -23,6 +23,7 @@ forced_uniform_after_cylinder_valid_streak: 5
 adaptive_quasirandom_sample_size : 64
 adaptive_start_radius : 1.0
 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)

src/ompl/geometric/planners/disassemblyrrt/src/MAB_RRT.cpp

@@ -183,6 +183,12 @@ void ompl::geometric::MAB_RRT::loadYAMLConfig(const std::string& yamlFilePath)
                              config["adaptive_shrink_step"].as<double>() : 0.1;
         adaptiveGrowStep_ = config["adaptive_grow_step"] ? 
                           config["adaptive_grow_step"].as<double>() : 0.1;
+        
+        // Maximum radius limit (from YAML, or computed from state space bounds if not provided)
+        if (config["adaptive_max_radius"])
+        {
+            adaptiveMaxRadius_ = config["adaptive_max_radius"].as<double>();
+        }
 
         // ----- Initial Uniform Check -----
         // Early exit optimization if problem is "easy" (high uniform validity)
@@ -288,11 +294,15 @@ void ompl::geometric::MAB_RRT::setup()
 
     // Compute maximum allowed burn-in radius from state space bounds
     // Do this after initializeArms() to ensure state space is fully set up
-    try {
-        adaptiveMaxRadius_ = computeMaxBurninRadius();
-    } catch (const std::exception& e) {
-        OMPL_WARN("Failed to compute max burn-in radius: %s. Using default.", e.what());
-        adaptiveMaxRadius_ = 1000.0;
+    // Only compute if not already set from YAML config
+    if (adaptiveMaxRadius_ == std::numeric_limits<double>::infinity())
+    {
+        try {
+            adaptiveMaxRadius_ = computeMaxBurninRadius();
+        } catch (const std::exception& e) {
+            OMPL_WARN("Failed to compute max burn-in radius: %s. Using default.", e.what());
+            adaptiveMaxRadius_ = 1000.0;
+        }
     }
 }
 

src/ompl/geometric/planners/disassemblyrrt/src/MAB_RRT_DEBUG.cpp

@@ -194,6 +194,13 @@
          adaptiveMaxExpectedValidityRate_ = config["adaptive_max_expected_validity_rate"].as<double>();
          adaptiveBurninMaxSteps_ = config["adaptive_burnin_max_steps"] ? 
                                    config["adaptive_burnin_max_steps"].as<int>() : 10;
+         
+         // Maximum radius limit (from YAML, or computed from state space bounds if not provided)
+         if (config["adaptive_max_radius"])
+         {
+             adaptiveMaxRadius_ = config["adaptive_max_radius"].as<double>();
+             OMPL_INFORM("DEBUG: Loaded adaptive_max_radius from YAML = %.10f", adaptiveMaxRadius_);
+         }
 
          // ----- Initial Uniform Check -----
          // Early exit optimization if problem is "easy" (high uniform validity)
@@ -296,10 +303,18 @@
 
      initializeArms();
 
-     // Compute maximum allowed burn-in radius from state space bounds
-     // Do this after initializeArms() to ensure state space is fully set up
-     adaptiveMaxRadius_ = computeMaxBurninRadius();
-     OMPL_INFORM("DEBUG: Computed max burn-in radius from state space bounds: %.6f", adaptiveMaxRadius_);
+    // Compute maximum allowed burn-in radius from state space bounds
+    // Do this after initializeArms() to ensure state space is fully set up
+    // Only compute if not already set from YAML config
+    if (adaptiveMaxRadius_ == std::numeric_limits<double>::infinity())
+    {
+        adaptiveMaxRadius_ = computeMaxBurninRadius();
+        OMPL_INFORM("DEBUG: Computed max burn-in radius from state space bounds: %.6f", adaptiveMaxRadius_);
+    }
+    else
+    {
+        OMPL_INFORM("DEBUG: Using adaptive_max_radius from YAML: %.6f", adaptiveMaxRadius_);
+    }
  }
 
  /**