Add configurable barrier update strategies for interior point methods (#144)

astomodynamics · web-flow · commit 71b966589a74 · 2025-07-19T02:52:45.000-04:00
* feat: Add configurable barrier update strategies for interior point methods

Implement three barrier parameter update strategies for IPDDP and MSIPDDP solvers:
- ADAPTIVE: Current adaptive strategy based on KKT progress (default)
- MONOTONIC: Fixed reduction factor for consistent barrier decrease
- IPOPT: IPOPT-style update with error threshold criterion

This enhancement provides users with flexibility to choose barrier update
strategies suited to their specific optimization problems while maintaining
backward compatibility with the default adaptive strategy.
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -14,6 +14,9 @@
 
 # CMakeLists.txt for the CDDP examples
 
+add_executable(test_barrier_strategies test_barrier_strategies.cpp)
+target_link_libraries(test_barrier_strategies cddp)
+
 add_executable(cddp_bicycle cddp_bicycle.cpp)
 target_link_libraries(cddp_bicycle cddp)
 
diff --git a/examples/test_barrier_strategies.cpp b/examples/test_barrier_strategies.cpp
@@ -0,0 +1,152 @@
+/*
+ Example demonstrating the three barrier update strategies for IPDDP and MSIPDDP
+*/
+
+#include <iostream>
+#include <memory>
+#include <string>
+#include <Eigen/Dense>
+
+#include "cddp.hpp"
+
+int main() 
+{
+    // Problem setup
+    const int horizon = 50;
+    const double timestep = 0.05;
+    const int state_dim = 3;
+    const int control_dim = 2;
+    
+    // Initial and goal states
+    Eigen::VectorXd X0(state_dim);
+    X0 << 0.0, 0.0, 0.0;  // x, y, theta
+    
+    Eigen::VectorXd Xg(state_dim);
+    Xg << 2.0, 2.0, 0.0;
+    
+    // Test each barrier strategy
+    std::vector<std::string> solvers = {"IPDDP", "MSIPDDP"};
+    std::vector<cddp::BarrierStrategy> strategies = {
+        cddp::BarrierStrategy::ADAPTIVE,
+        cddp::BarrierStrategy::MONOTONIC,
+        cddp::BarrierStrategy::IPOPT
+    };
+    std::vector<std::string> strategy_names = {"ADAPTIVE", "MONOTONIC", "IPOPT"};
+    
+    for (const auto& solver_name : solvers) 
+    {
+        std::cout << "\n========================================\n";
+        std::cout << "Testing " << solver_name << " Solver\n";
+        std::cout << "========================================\n";
+        
+        for (size_t i = 0; i < strategies.size(); ++i) 
+        {
+            std::cout << "\n--- Barrier Strategy: " << strategy_names[i] << " ---\n";
+            
+            // Create CDDP solver first
+            cddp::CDDP cddp_solver(X0, Xg, horizon, timestep);
+            
+            // Create and set dynamics model
+            auto dynamics = std::make_unique<cddp::Unicycle>(timestep);
+            cddp_solver.setDynamicalSystem(std::move(dynamics));
+            
+            // Create and set objective
+            Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(state_dim, state_dim);
+            Q(0, 0) = 10.0;  // x
+            Q(1, 1) = 10.0;  // y
+            Q(2, 2) = 1.0;   // theta
+            
+            Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim);
+            R(0, 0) = 0.1;  // linear velocity
+            R(1, 1) = 0.1;  // angular velocity
+            
+            Eigen::MatrixXd Qf = 100.0 * Q;
+            
+            std::vector<Eigen::VectorXd> empty_reference;
+            auto objective = std::make_unique<cddp::QuadraticObjective>(
+                Q, R, Qf, Xg, empty_reference, timestep);
+            cddp_solver.setObjective(std::move(objective));
+            
+            // Add constraints
+            Eigen::VectorXd u_upper(control_dim);
+            u_upper << 1.0, 2.0;  // max linear vel, max angular vel
+            
+            cddp_solver.addPathConstraint("ControlConstraint",
+                std::make_unique<cddp::ControlConstraint>(u_upper));
+            
+            // State bounds (keep robot in a region)
+            Eigen::VectorXd x_lower(state_dim), x_upper_state(state_dim);
+            x_lower << -0.5, -0.5, -M_PI;
+            x_upper_state << 2.5, 2.5, M_PI;
+            
+            cddp_solver.addPathConstraint("StateConstraint",
+                std::make_unique<cddp::StateConstraint>(x_lower, x_upper_state));
+            
+            // Configure options
+            cddp::CDDPOptions opts;
+            opts.max_iterations = 100;
+            opts.tolerance = 1e-4;
+            opts.verbose = false;
+            opts.debug = true;  // Enable debug output to see barrier updates
+            opts.return_iteration_info = true;
+            
+            // Set barrier strategy
+            if (solver_name == "IPDDP") {
+                opts.ipddp.barrier.strategy = strategies[i];
+                opts.ipddp.barrier.mu_initial = 1.0;
+                opts.ipddp.barrier.mu_update_factor = 0.2;
+                opts.ipddp.barrier.mu_update_power = 1.5;
+            } else {
+                opts.msipddp.barrier.strategy = strategies[i];
+                opts.msipddp.barrier.mu_initial = 1.0;
+                opts.msipddp.barrier.mu_update_factor = 0.2;
+                opts.msipddp.barrier.mu_update_power = 1.5;
+            }
+            
+            cddp_solver.setOptions(opts);
+            
+            // Initialize with straight line trajectory
+            std::vector<Eigen::VectorXd> X_init(horizon + 1);
+            std::vector<Eigen::VectorXd> U_init(horizon);
+            
+            for (int t = 0; t <= horizon; ++t) {
+                double alpha = static_cast<double>(t) / horizon;
+                X_init[t] = (1.0 - alpha) * X0 + alpha * Xg;
+            }
+            
+            for (int t = 0; t < horizon; ++t) {
+                U_init[t] = Eigen::VectorXd::Zero(control_dim);
+            }
+            
+            cddp_solver.setInitialTrajectory(X_init, U_init);
+            
+            // Solve
+            cddp::CDDPSolution solution = cddp_solver.solve(solver_name);
+            
+            // Print results
+            auto status_message = std::any_cast<std::string>(solution.at("status_message"));
+            auto iterations = std::any_cast<int>(solution.at("iterations_completed"));
+            auto solve_time = std::any_cast<double>(solution.at("solve_time_ms"));
+            auto final_cost = std::any_cast<double>(solution.at("final_objective"));
+            
+            std::cout << "Status: " << status_message << "\n";
+            std::cout << "Iterations: " << iterations << "\n";
+            std::cout << "Final cost: " << final_cost << "\n";
+            std::cout << "Solve time: " << solve_time << " ms\n";
+            
+            // Extract final barrier parameter
+            try {
+                double final_mu = std::any_cast<double>(solution.at("final_barrier_parameter_mu"));
+                std::cout << "Final barrier μ: " << final_mu << "\n";
+            } catch (const std::exception& e) {
+                std::cout << "Final barrier μ: Not available\n";
+            }
+        }
+    }
+    
+    std::cout << "\n========================================\n";
+    std::cout << "Test completed successfully!\n";
+    std::cout << "========================================\n";
+    
+    return 0;
+}
diff --git a/include/cddp-cpp/cddp_core/options.hpp b/include/cddp-cpp/cddp_core/options.hpp
@@ -22,6 +22,16 @@
 
 namespace cddp
 {
+    /**
+     * @brief Barrier parameter update strategy for interior point methods.
+     */
+    enum class BarrierStrategy
+    {
+        ADAPTIVE,    ///< Adaptive strategy based on KKT progress (default)
+        MONOTONIC,   ///< Monotonic decrease with fixed reduction factor
+        IPOPT        ///< IPOPT-style adaptive barrier update
+    };
+
     /**
      * @brief Options for the line search procedure.
      *
@@ -71,6 +81,8 @@ namespace cddp
         double min_fraction_to_boundary =
             0.99; ///< Minimum fraction to boundary for primal/dual step calculation
                   ///< (tau).
+        BarrierStrategy strategy = 
+            BarrierStrategy::ADAPTIVE; ///< Barrier parameter update strategy.
     };
 
     /**
diff --git a/src/cddp_core/ipddp_solver.cpp b/src/cddp_core/ipddp_solver.cpp
@@ -1838,56 +1838,106 @@ namespace cddp
       return; // No constraints case - no barrier update needed
     }
 
-    // Compute termination metric from current infeasibility metrics with IPOPT-style scaling
-    double scaled_inf_du = computeScaledDualInfeasibility(context);
-    double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_});
-
-    // More aggressive barrier parameter update strategy
-    double barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0);
-    
-    if (termination_metric <= barrier_update_threshold)
+    // Select barrier update strategy
+    switch (barrier_opts.strategy)
     {
-      // Adaptive barrier reduction strategy
-      double reduction_factor = barrier_opts.mu_update_factor;
-
-      if (mu_ > 1e-12)
+      case BarrierStrategy::MONOTONIC:
       {
-        double kkt_progress_ratio = termination_metric / mu_;
-
-        // Very aggressive reduction for good KKT satisfaction
-        if (kkt_progress_ratio < 0.01)
-        {
-          reduction_factor = barrier_opts.mu_update_factor * 0.1;
-        }
-        // Aggressive reduction if we're significantly satisfying KKT conditions
-        else if (kkt_progress_ratio < 0.1)
+        // Monotonic decrease: always reduce by fixed factor
+        mu_ = std::max(barrier_opts.mu_min_value, 
+                       barrier_opts.mu_update_factor * mu_);
+        resetFilter(context);
+        if (options.debug)
         {
-          reduction_factor = barrier_opts.mu_update_factor * 0.3;
+          std::cout << "[IPDDP Barrier] Monotonic update: μ = " 
+                    << std::scientific << std::setprecision(2) << mu_ << std::endl;
         }
-        // Moderate reduction if we're moderately satisfying KKT conditions
-        else if (kkt_progress_ratio < 0.5)
+        break;
+      }
+      
+      case BarrierStrategy::IPOPT:
+      {
+        // IPOPT-style barrier update
+        double scaled_inf_du = computeScaledDualInfeasibility(context);
+        double error_k = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_});
+        
+        // IPOPT uses: μ_new = max(ε_tol/10, min(κ_μ * μ, μ^θ_μ))
+        // where update happens when error_k ≤ κ_ε * μ
+        double kappa_epsilon = 10.0; // IPOPT default
+        
+        if (error_k <= kappa_epsilon * mu_)
         {
-          reduction_factor = barrier_opts.mu_update_factor * 0.6;
+          double new_mu_linear = barrier_opts.mu_update_factor * mu_;
+          double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power);
+          mu_ = std::max(options.tolerance / 10.0,
+                         std::min(new_mu_linear, new_mu_superlinear));
+          resetFilter(context);
+          if (options.debug)
+          {
+            std::cout << "[IPDDP Barrier] IPOPT update: error = " 
+                      << std::scientific << std::setprecision(2) << error_k 
+                      << " ≤ " << kappa_epsilon << " * μ = " << kappa_epsilon * mu_
+                      << " → μ = " << mu_ << std::endl;
+          }
         }
-        // Standard reduction otherwise
+        break;
       }
+      
+      case BarrierStrategy::ADAPTIVE:
+      default:
+      {
+        // Current adaptive strategy (default)
+        double scaled_inf_du = computeScaledDualInfeasibility(context);
+        double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_});
 
-      // Update barrier parameter with bounds
-      double new_mu_linear = reduction_factor * mu_;
-      double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power);
+        // More aggressive barrier parameter update strategy
+        double barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0);
+        
+        if (termination_metric <= barrier_update_threshold)
+        {
+          // Adaptive barrier reduction strategy
+          double reduction_factor = barrier_opts.mu_update_factor;
 
-      mu_ = std::max(options.tolerance / 100.0,
-                     std::min(new_mu_linear, new_mu_superlinear));
+          if (mu_ > 1e-12)
+          {
+            double kkt_progress_ratio = termination_metric / mu_;
 
-      // Reset filter when barrier parameter changes
-      resetFilter(context);
+            // Very aggressive reduction for good KKT satisfaction
+            if (kkt_progress_ratio < 0.01)
+            {
+              reduction_factor = barrier_opts.mu_update_factor * 0.1;
+            }
+            // Aggressive reduction if we're significantly satisfying KKT conditions
+            else if (kkt_progress_ratio < 0.1)
+            {
+              reduction_factor = barrier_opts.mu_update_factor * 0.3;
+            }
+            // Moderate reduction if we're moderately satisfying KKT conditions
+            else if (kkt_progress_ratio < 0.5)
+            {
+              reduction_factor = barrier_opts.mu_update_factor * 0.6;
+            }
+            // Standard reduction otherwise
+          }
 
-      if (options.debug)
-      {
-        std::cout << "[IPDDP Barrier] Termination metric: " << std::scientific << std::setprecision(2)
-                  << termination_metric << " (scaled inf_du: " << scaled_inf_du 
-                  << ", inf_pr: " << context.inf_pr_ << ", inf_comp: " << context.inf_comp_ 
-                  << ") → μ: " << mu_ << std::endl;
+          // Update barrier parameter with bounds
+          double new_mu_linear = reduction_factor * mu_;
+          double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power);
+
+          mu_ = std::max(options.tolerance / 100.0,
+                         std::min(new_mu_linear, new_mu_superlinear));
+
+          // Reset filter when barrier parameter changes
+          resetFilter(context);
+
+          if (options.debug)
+          {
+            std::cout << "[IPDDP Barrier] Adaptive update: termination metric = " 
+                      << std::scientific << std::setprecision(2) << termination_metric 
+                      << " → μ = " << mu_ << std::endl;
+          }
+        }
+        break;
       }
     }
   }
diff --git a/src/cddp_core/msipddp_solver.cpp b/src/cddp_core/msipddp_solver.cpp
