diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 9a54f409..86cc96aa 100644 --- 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 new file mode 100644 index 00000000..0c2cb47c --- /dev/null +++ b/examples/test_barrier_strategies.cpp @@ -0,0 +1,152 @@ +/* + Example demonstrating the three barrier update strategies for IPDDP and MSIPDDP +*/ + +#include +#include +#include +#include + +#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 solvers = {"IPDDP", "MSIPDDP"}; + std::vector strategies = { + cddp::BarrierStrategy::ADAPTIVE, + cddp::BarrierStrategy::MONOTONIC, + cddp::BarrierStrategy::IPOPT + }; + std::vector 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(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 empty_reference; + auto objective = std::make_unique( + 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(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(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 X_init(horizon + 1); + std::vector U_init(horizon); + + for (int t = 0; t <= horizon; ++t) { + double alpha = static_cast(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(solution.at("status_message")); + auto iterations = std::any_cast(solution.at("iterations_completed")); + auto solve_time = std::any_cast(solution.at("solve_time_ms")); + auto final_cost = std::any_cast(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(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; +} \ No newline at end of file diff --git a/include/cddp-cpp/cddp_core/options.hpp b/include/cddp-cpp/cddp_core/options.hpp index 9e7ad7c0..1cb85432 100644 --- 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 index d9873a66..08baf2e2 100644 --- 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 index 0e199346..9d2e8bdb 100644 --- a/src/cddp_core/msipddp_solver.cpp +++ b/src/cddp_core/msipddp_solver.cpp @@ -2253,81 +2253,129 @@ 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_}); - - // Adaptive barrier parameter update strategy - // Use different thresholds based on current progress - double barrier_update_threshold; - - // If mu is already small, use a more relaxed threshold - if (mu_ < 1e-5) - { - // For small mu, update if we've made any reasonable progress - barrier_update_threshold = std::max(termination_metric * 10.0, mu_ * 100.0); - } - else - { - // Standard threshold for larger mu values - barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0); - } - - // Also consider updating if forward pass succeeded but with very small cost change NOTE: Hand tuning - bool slow_progress = forward_pass_success && context.alpha_pr_ > 0 && - (termination_metric < 1e-3); // Only when reasonably feasible - - if (termination_metric <= barrier_update_threshold || slow_progress) + // 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) + // 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.1; + std::cout << "[MSIPDDP Barrier] Monotonic update: μ = " + << std::scientific << std::setprecision(2) << mu_ << std::endl; } - // Aggressive reduction if we're significantly satisfying KKT conditions - else if (kkt_progress_ratio < 0.1) + 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(κ_μ * μ, μ^θ_μ)) + double kappa_epsilon = 10.0; // IPOPT default + + if (error_k <= kappa_epsilon * mu_) { - reduction_factor = barrier_opts.mu_update_factor * 0.3; + 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 << "[MSIPDDP Barrier] IPOPT update: error = " + << std::scientific << std::setprecision(2) << error_k + << " ≤ " << kappa_epsilon << " * μ = " << kappa_epsilon * mu_ + << " → μ = " << mu_ << std::endl; + } } - // Moderate reduction if we're moderately satisfying KKT conditions - else if (kkt_progress_ratio < 0.5) + 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_}); + + // Adaptive barrier parameter update strategy + double barrier_update_threshold; + + // If mu is already small, use a more relaxed threshold + if (mu_ < 1e-5) { - reduction_factor = barrier_opts.mu_update_factor * 0.6; + // For small mu, update if we've made any reasonable progress + barrier_update_threshold = std::max(termination_metric * 10.0, mu_ * 100.0); + } + else + { + // Standard threshold for larger mu values + barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0); } - // Standard reduction otherwise - } - // Update barrier parameter with bounds - double new_mu_linear = reduction_factor * mu_; - double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power); + // Also consider updating if forward pass succeeded but with very small cost change NOTE: Hand tuning + bool slow_progress = forward_pass_success && context.alpha_pr_ > 0 && + (termination_metric < 1e-3); // Only when reasonably feasible - // Choose the more aggressive reduction when progress is slow - if (slow_progress && mu_ > options.tolerance) - { - mu_ = std::min(new_mu_linear, new_mu_superlinear); - } - else - { - mu_ = std::max(options.tolerance / 100.0, - std::min(new_mu_linear, new_mu_superlinear)); - } + if (termination_metric <= barrier_update_threshold || slow_progress) + { + // Adaptive barrier reduction strategy + double reduction_factor = barrier_opts.mu_update_factor; - // Reset filter when barrier parameter changes - resetFilter(context); + if (mu_ > 1e-12) + { + double kkt_progress_ratio = termination_metric / mu_; - if (options.debug) - { - std::cout << "[MSIPDDP 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; + // 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 + } + + // Update barrier parameter with bounds + double new_mu_linear = reduction_factor * mu_; + double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power); + + // Choose the more aggressive reduction when progress is slow + if (slow_progress && mu_ > options.tolerance) + { + mu_ = std::min(new_mu_linear, new_mu_superlinear); + } + else + { + 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 << "[MSIPDDP Barrier] Adaptive update: termination metric = " + << std::scientific << std::setprecision(2) << termination_metric + << " → μ = " << mu_ << std::endl; + } + } + break; } } }