Add configurable barrier update strategies for interior point methods

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)
 

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;
+}

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.
     };
 
     /**

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;
       }
     }
   }