diff --git a/CMakeLists.txt b/CMakeLists.txt index ff61aac..42beca5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -196,7 +196,7 @@ set(cddp_core_srcs src/cddp_core/boxqp.cpp src/cddp_core/qp_solver.cpp src/cddp_core/cddp_core.cpp - src/cddp_core/clddp_core.cpp + src/cddp_core/asddp_core.cpp src/cddp_core/logddp_core.cpp src/cddp_core/torch_dynamical_system.cpp ) diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp index d0319e8..84e8d98 100644 --- a/examples/cddp_car.cpp +++ b/examples/cddp_car.cpp @@ -189,6 +189,9 @@ int main() { options.grad_tolerance = 1e-4; options.regularization_type = "control"; // options.regularization_control = 1.0; + options.debug = true; + options.use_parallel = true; + options.num_threads = 10; cddp_solver.setOptions(options); // Initialize with random controls @@ -221,51 +224,51 @@ int main() { cddp_solver.setInitialTrajectory(X, U); // Solve the problem - // cddp::CDDPSolution solution = cddp_solver.solve(); - cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); + cddp::CDDPSolution solution = cddp_solver.solve(); + // cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP(); - // // Extract solution trajectories - // auto X_sol = solution.state_sequence; - // auto U_sol = solution.control_sequence; - // auto t_sol = solution.time_sequence; - - // // Prepare trajectory data - // std::vector x_hist, y_hist; - // for(const auto& x : X_sol) { - // x_hist.push_back(x(0)); - // y_hist.push_back(x(1)); - // } - - // // Car dimensions - // double car_length = 2.1; - // double car_width = 0.9; - - // // Create animation - // Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim); - // for(size_t i = 0; i < X_sol.size(); i++) { - // animation.newFrame(); + // Extract solution trajectories + auto X_sol = solution.state_sequence; + auto U_sol = solution.control_sequence; + auto t_sol = solution.time_sequence; + + // Prepare trajectory data + std::vector x_hist, y_hist; + for(const auto& x : X_sol) { + x_hist.push_back(x(0)); + y_hist.push_back(x(1)); + } + + // Car dimensions + double car_length = 2.1; + double car_width = 0.9; + + // Create animation + Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim); + for(size_t i = 0; i < X_sol.size(); i++) { + animation.newFrame(); - // // Plot full trajectory - // plt::plot(x_hist, y_hist, "b-"); + // Plot full trajectory + plt::plot(x_hist, y_hist, "b-"); - // // Plot goal configuration - // plotCarBox(goal_state, empty_control, car_length, car_width, "r"); + // Plot goal configuration + plotCarBox(goal_state, empty_control, car_length, car_width, "r"); - // // Plot current car position - // plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k"); + // Plot current car position + plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k"); - // // Plot settings - // plt::grid(true); - // // plt::axis("equal"); - // plt::xlim(-4, 4); - // plt::ylim(-4, 4); + // Plot settings + plt::grid(true); + // plt::axis("equal"); + plt::xlim(-4, 4); + plt::ylim(-4, 4); - // animation.saveFrame(i); - // } + animation.saveFrame(i); + } - // // Create the final GIF - // animation.createGif("car_parking.gif"); + // Create the final GIF + animation.createGif("car_parking.gif"); return 0; } diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp index 660eb7c..ad06fd0 100644 --- a/examples/cddp_lti_system.cpp +++ b/examples/cddp_lti_system.cpp @@ -157,8 +157,7 @@ int main() { cddp_solver.setInitialTrajectory(X, U); // Solve using different CDDP variants - // cddp::CDDPSolution solution = cddp_solver.solve(); - cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); + cddp::CDDPSolution solution = cddp_solver.solve(); // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP(); // Extract solution trajectories diff --git a/include/cddp-cpp/cddp_core/boxqp.hpp b/include/cddp-cpp/cddp_core/boxqp.hpp index 400b2d0..959df6f 100644 --- a/include/cddp-cpp/cddp_core/boxqp.hpp +++ b/include/cddp-cpp/cddp_core/boxqp.hpp @@ -27,13 +27,13 @@ namespace cddp */ struct BoxQPOptions { - int maxIter = 100; ///< Maximum number of iterations - double minGrad = 1e-8; ///< Minimum norm of non-fixed gradient - double minRelImprove = 1e-8; ///< Minimum relative improvement - double stepDec = 0.6; ///< Factor for decreasing stepsize - double minStep = 1e-22; ///< Minimal stepsize for linesearch - double armijo = 0.1; ///< Armijo parameter - bool verbose = false; ///< Print debug info + int max_iterations = 100; ///< Maximum number of iterations + double min_grad = 1e-8; ///< Minimum norm of non-fixed gradient + double min_rel_improve = 1e-8; ///< Minimum relative improvement + double step_dec = 0.6; ///< Factor for decreasing stepsize + double min_step = 1e-22; ///< Minimal stepsize for linesearch + double armijo = 0.1; ///< Armijo parameter + bool verbose = false; ///< Print debug info }; /** @@ -46,9 +46,8 @@ enum class BoxQPStatus MAX_ITER_EXCEEDED = 1, ///< Maximum iterations exceeded MAX_LS_EXCEEDED = 2, ///< Maximum line search iterations exceeded NO_BOUNDS = 3, ///< No bounds, returning Newton point - SMALL_IMPROVEMENT = 4, ///< Improvement smaller than tolerance - SMALL_GRADIENT = 5, ///< Gradient norm smaller than tolerance - ALL_CLAMPED = 6 ///< All dimensions are clamped + SUCCESS = 4, ///< Converged successfully + ALL_CLAMPED = 5 ///< All dimensions are clamped }; /** diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index dd99603..90f8526 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -30,6 +30,7 @@ #include "cddp_core/dynamical_system.hpp" #include "cddp_core/objective.hpp" #include "cddp_core/constraint.hpp" +#include "cddp_core/boxqp.hpp" namespace cddp { @@ -53,9 +54,6 @@ struct CDDPOptions { double relaxation_coeff = 1.0; // Relaxation for log-barrier method int barrier_order = 2; // Order for log-barrier method - // Active set method - double active_set_tolerance = 1e-6; // Tolerance for active set method - // Regularization options std::string regularization_type = "control"; // different regularization types: ["none", "control", "state", "both"] @@ -77,6 +75,15 @@ struct CDDPOptions { bool is_ilqr = true; // Option for iLQR bool use_parallel = true; // Option for parallel computation int num_threads = max_line_search_iterations; // Number of threads to use + + // Boxqp options + double boxqp_max_iterations = 100; // Maximum number of iterations for boxqp + double boxqp_min_grad = 1e-8; // Minimum gradient norm for boxqp + double boxqp_min_rel_improve = 1e-8; // Minimum relative improvement for boxqp + double boxqp_step_dec = 0.6; // Step decrease factor for boxqp + double boxqp_min_step = 1e-22; // Minimum step size for boxqp + double boxqp_armijo = 0.1; // Armijo parameter for boxqp + bool boxqp_verbose = false; // Print debug info for boxqp }; struct CDDPSolution { @@ -106,7 +113,10 @@ class CDDP { CDDP(const Eigen::VectorXd& initial_state, const Eigen::VectorXd& reference_state, int horizon, - double timestep); + double timestep, + std::unique_ptr system = nullptr, + std::unique_ptr objective = nullptr, + const CDDPOptions& options = CDDPOptions()); // Accessor methods @@ -169,7 +179,7 @@ class CDDP { * @param X state trajectory * @param U control trajectory */ - void setInitialTrajectory(const std::vector& X, const std::vector& U) { X_ = X; U_ = U; } + void setInitialTrajectory(const std::vector& X, const std::vector& U); /** * @brief Add a constraint to the problem @@ -195,14 +205,6 @@ class CDDP { // and does not modify 'constraint_set_' auto it = constraint_set_.find(name); - // Special case for ControlBoxConstraint - must exist - if (std::is_same::value) { - if (it == constraint_set_.end()) { - throw std::runtime_error("ControlBoxConstraint not found"); - } - return dynamic_cast(it->second.get()); - } - // For other constraints, return nullptr if not found if (it == constraint_set_.end()) { return nullptr; @@ -223,62 +225,74 @@ class CDDP { return constraint_set_; } + // Initialization methods + void initializeCDDP(); + // Solve the problem CDDPSolution solve(); - CDDPSolution solveCLDDP(); CDDPSolution solveLogCDDP(); + CDDPSolution solveASCDDP(); +private: + // Solver methods + ForwardPassResult solveForwardPass(double alpha); + bool solveBackwardPass(); + + ForwardPassResult solveLogCDDPForwardPass(double alpha); + bool solveLogCDDPBackwardPass(); + + ForwardPassResult solveASCDDPForwardPass(double alpha); + bool solveASCDDPBackwardPass(); + + // Helper methods + bool checkConvergence(double J_new, double J_old, double dJ, double expected_dV, double gradient_norm); + + void printSolverInfo(); + + void printOptions(const CDDPOptions& options); + + void printIteration(int iter, double cost, double lagrangian, + double grad_norm, double lambda_state, + double lambda_control, double alpha); + + void printSolution(const CDDPSolution& solution); private: + bool initialized_ = false; // Initialization flag // Problem Data std::unique_ptr system_; // Eigen-based dynamical system // std::unique_ptr torch_system_; // Torch-based dynamical system (learned dynamics model) + std::unique_ptr objective_; // Objective function + std::map> constraint_set_; + std::unique_ptr log_barrier_; Eigen::VectorXd initial_state_; // Initial state of the system Eigen::VectorXd reference_state_; // Desired reference state int horizon_; // Time horizon for the problem double timestep_; // Time step for the problem - - // Options CDDPOptions options_; // Options for the solver // Intermediate trajectories std::vector X_; // State trajectory std::vector U_; // Control trajectory - // Intermediate cost + // Cost and Lagrangian double J_; // Cost double dJ_; // Cost improvement double L_; // Lagrangian double dL_; // Lagrangian improvement - double optimality_gap_ = 1e+10; - double expected_; - double reduction_; + Eigen::VectorXd dV_; // Line search double alpha_; // Line search step size std::vector alphas_; - // Cost function - std::unique_ptr objective_; // Objective function - - // Constraints - std::map> constraint_set_; - // Feedforward and feedback gains std::vector k_; std::vector K_; - // Intermediate value function - Eigen::VectorXd dV_; - std::vector V_X_; - std::vector V_XX_; - // Q-function matrices std::vector Q_UU_; std::vector Q_UX_; - std::vector Q_U_; - - // Log-barrier method - std::unique_ptr log_barrier_; + std::vector Q_U_; // Regularization parameters double regularization_state_; @@ -286,131 +300,11 @@ class CDDP { double regularization_control_; double regularization_control_step_; - // Initialization methods - void initializeCDDP(); // Initialize the CDDP solver - - // Solver methods - bool solveForwardPass(); - ForwardPassResult solveForwardPassIteration(double alpha); - bool solveBackwardPass(); - - ForwardPassResult solveCLDDPForwardPass(double alpha); - bool solveCLDDPBackwardPass(); - - ForwardPassResult solveLogCDDPForwardPass(double alpha); - bool solveLogCDDPBackwardPass(); - - // Helper methods - bool checkConvergence(double J_new, double J_old, double dJ, double expected_dV, double gradient_norm); + // Boxqp solver + BoxQPOptions boxqp_options_; + BoxQPSolver boxqp_solver_; - - - void printSolverInfo() - { - std::cout << "\n"; - std::cout << "\033[34m"; // Set text color to blue - std::cout << "+---------------------------------------------------------+" << std::endl; - std::cout << "| ____ ____ ____ ____ _ ____ |" << std::endl; - std::cout << "| / ___| _ \\| _ \\| _ \\ (_)_ __ / ___| _ _ |" << std::endl; - std::cout << "| | | | | | | | | | |_) | | | '_ \\ | | _| |_ _| |_ |" << std::endl; - std::cout << "| | |___| |_| | |_| | __/ | | | | | | |__|_ _|_ _| |" << std::endl; - std::cout << "| \\____|____/|____/|_| |_|_| |_| \\____||_| |_| |" << std::endl; - std::cout << "+---------------------------------------------------------+" << std::endl; - std::cout << "\n"; - std::cout << "Constrained Differential Dynamic Programming\n"; - std::cout << "Author: Tomo Sasaki (@astomodynamics)\n"; - std::cout << "----------------------------------------------------------\n"; - std::cout << "\033[0m"; // Reset text color - std::cout << "\n"; - } - - void printOptions(const CDDPOptions &options) - { - std::cout << "\n========================================\n"; - std::cout << " CDDP Options\n"; - std::cout << "========================================\n"; - - std::cout << "Cost Tolerance: " << std::setw(10) << options.cost_tolerance << "\n"; - std::cout << "Grad Tolerance: " << std::setw(10) << options.grad_tolerance << "\n"; - std::cout << "Max Iterations: " << std::setw(10) << options.max_iterations << "\n"; - std::cout << "Max CPU Time: " << std::setw(10) << options.max_cpu_time << "\n"; - - std::cout << "\nLine Search:\n"; - std::cout << " Max Iterations: " << std::setw(5) << options.max_line_search_iterations << "\n"; - std::cout << " Backtracking Coeff: " << std::setw(5) << options.backtracking_coeff << "\n"; - std::cout << " Backtracking Min: " << std::setw(5) << options.backtracking_min << "\n"; - std::cout << " Backtracking Factor: " << std::setw(5) << options.backtracking_factor << "\n"; - - std::cout << "\nLog-Barrier:\n"; - std::cout << " Barrier Coeff: " << std::setw(5) << options.barrier_coeff << "\n"; - std::cout << " Barrier Factor: " << std::setw(5) << options.barrier_factor << "\n"; - std::cout << " Barrier Tolerance: " << std::setw(5) << options.barrier_tolerance << "\n"; - std::cout << " Relaxation Coeff: " << std::setw(5) << options.relaxation_coeff << "\n"; - - std::cout << "\nRegularization:\n"; - std::cout << " Regularization Type: " << options.regularization_type << "\n"; - std::cout << " Regularization State: " << std::setw(5) << options.regularization_state << "\n"; - std::cout << " Regularization State Step: " << std::setw(5) << options.regularization_state_step << "\n"; - std::cout << " Regularization State Max: " << std::setw(5) << options.regularization_state_max << "\n"; - std::cout << " Regularization State Min: " << std::setw(5) << options.regularization_state_min << "\n"; - std::cout << " Regularization State Factor: " << std::setw(5) << options.regularization_state_factor << "\n"; - - std::cout << " Regularization Control: " << std::setw(5) << options.regularization_control << "\n"; - std::cout << " Regularization Control Step: " << std::setw(5) << options.regularization_control_step << "\n"; - std::cout << " Regularization Control Max: " << std::setw(5) << options.regularization_control_max << "\n"; - std::cout << " Regularization Control Min: " << std::setw(5) << options.regularization_control_min << "\n"; - std::cout << " Regularization Control Factor: " << std::setw(5) << options.regularization_control_factor << "\n"; - - std::cout << "\nOther:\n"; - std::cout << " Print Iterations: " << (options.verbose ? "Yes" : "No") << "\n"; - std::cout << " iLQR: " << (options.is_ilqr ? "Yes" : "No") << "\n"; - std::cout << " Use Parallel: " << (options.use_parallel ? "Yes" : "No") << "\n"; - std::cout << " Num Threads: " << options.num_threads << "\n"; - - std::cout << "========================================\n\n"; - } - - void printIteration(int iter, double cost, double lagrangian, double grad_norm, - double lambda_state, double lambda_control, double step_size) - { - // Print header for better readability every 10 iterations - if (iter % 10 == 0) - { - std::cout << std::setw(10) << "Iteration" - << std::setw(15) << "Objective" - << std::setw(15) << "Lagrangian" - << std::setw(15) << "Grad Norm" - << std::setw(15) << "Step Size" - << std::setw(15) << "Reg (State)" - << std::setw(15) << "Reg (Control)" - << std::endl; - std::cout << std::string(95, '-') << std::endl; - } - - // Print iteration details - std::cout << std::setw(10) << iter - << std::setw(15) << std::setprecision(6) << cost - << std::setw(15) << std::setprecision(6) << lagrangian - << std::setw(15) << std::setprecision(6) << grad_norm - << std::setw(15) << std::setprecision(6) << step_size - << std::setw(15) << std::setprecision(6) << lambda_state - << std::setw(15) << std::setprecision(6) << lambda_control - << std::endl; - } - - void printSolution(const CDDPSolution &solution) - { - std::cout << "\n========================================\n"; - std::cout << " CDDP Solution\n"; - std::cout << "========================================\n"; - - std::cout << "Converged: " << (solution.converged ? "Yes" : "No") << "\n"; - std::cout << "Iterations: " << solution.iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(4) << solution.solve_time << " micro sec\n"; - std::cout << "Final Cost: " << std::setprecision(6) << solution.cost_sequence.back() << "\n"; // Assuming cost_sequence is not empty - - std::cout << "========================================\n\n"; - } + double optimality_gap_ = 1e+10; }; -} +} // namespace cddp #endif // CDDP_CDDP_CORE_HPP \ No newline at end of file diff --git a/src/cddp_core/clddp_core.cpp b/src/cddp_core/asddp_core.cpp similarity index 53% rename from src/cddp_core/clddp_core.cpp rename to src/cddp_core/asddp_core.cpp index 292f2c5..bf3e0de 100644 --- a/src/cddp_core/clddp_core.cpp +++ b/src/cddp_core/asddp_core.cpp @@ -15,46 +15,76 @@ */ #include // For std::cout, std::cerr -#include // For std::setw -#include // For std::unique_ptr -#include // For std::map -#include // For std::log +#include // For std::setw +#include // For std::unique_ptr +#include // For std::map +#include // For std::log #include -#include // For timing +#include // For timing #include // For parallel execution policies #include // For multi-threading -#include // For multi-threading +#include // For multi-threading #include "cddp_core/cddp_core.hpp" #include "cddp_core/helper.hpp" #include "cddp_core/boxqp.hpp" +#include "cddp_core/qp_solver.hpp" -namespace cddp { +namespace cddp +{ +// Solve the problem +CDDPSolution CDDP::solveASCDDP() +{ + // Initialize if not done + if (!initialized_) { + initializeCDDP(); + } -CDDPSolution CDDP::solveCLDDP() { - // Initialize CDDP solver - initializeCDDP(); - - if (options_.verbose) { - printOptions(options_); + if (!initialized_) { + std::cerr << "CDDP: Initialization failed" << std::endl; + throw std::runtime_error("CDDP: Initialization failed"); } - // Initialize solution + // Prepare solution struct CDDPSolution solution; solution.converged = false; + solution.alpha = alpha_; + solution.iterations = 0; + solution.solve_time = 0.0; solution.time_sequence.reserve(horizon_ + 1); for (int t = 0; t <= horizon_; ++t) { solution.time_sequence.push_back(timestep_ * t); } + solution.control_sequence.reserve(horizon_); + solution.state_sequence.reserve(horizon_ + 1); solution.cost_sequence.reserve(options_.max_iterations); // Reserve space for efficiency - - // Evaluate initial cost - J_ = objective_->evaluate(X_, U_); + solution.lagrangian_sequence.reserve(options_.max_iterations); // Reserve space for efficiency solution.cost_sequence.push_back(J_); + // Evaluate Lagrangian + L_ = J_; + // // Loop over horizon + // for (int t = 0; t < 1; ++t) { + // // Evaluate state constraint violation + // for (const auto& constraint : constraint_set_) { + // if (constraint.first == "ControlBoxConstraint") { + // L_ += getLogBarrierCost(*constraint.second, X_[t], U_[t], barrier_coeff_, options_.relaxation_coeff); + // // Eigen::VectorXd constraint_violation = constraint.second->evaluate(X_[t], U_[t]); + // // if (constraint_violation.minCoeff() < 0) { + // // std::cerr << "CDDP: Constraint violation at time " << t << std::endl; + // // std::cerr << "Constraint violation: " << constraint_violation.transpose() << std::endl; + // // throw std::runtime_error("Constraint violation"); + // // } + // } + + // } + // } + + solution.lagrangian_sequence.push_back(L_); + if (options_.verbose) { - printIteration(0, J_, 0.0, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information + printIteration(0, J_, J_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information } // Start timer @@ -65,6 +95,7 @@ CDDPSolution CDDP::solveCLDDP() { while (iter < options_.max_iterations) { ++iter; + solution.iterations = iter; // Check maximum CPU time if (options_.max_cpu_time > 0) { @@ -80,10 +111,12 @@ CDDPSolution CDDP::solveCLDDP() { // 1. Backward pass: Solve Riccati recursion to compute optimal control law bool backward_pass_success = false; - while (!backward_pass_success) { - backward_pass_success = solveCLDDPBackwardPass(); + while (!backward_pass_success) + { + backward_pass_success = solveASCDDPBackwardPass(); - if (!backward_pass_success) { + if (!backward_pass_success) + { std::cerr << "CDDP: Backward pass failed" << std::endl; // Increase regularization @@ -92,16 +125,15 @@ CDDPSolution CDDP::solveCLDDP() { regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); regularization_control_ = std::max(regularization_control_ * regularization_control_step_, options_.regularization_control_min); - if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) { - if (options_.verbose) { - std::cerr << "CDDP: Regularization limit reached" << std::endl; - } + if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) + { + std::cerr << "CDDP: Regularization limit reached" << std::endl; break; // Exit if regularization limit reached } continue; // Continue if backward pass fails } } - + // Check termination due to small cost improvement if (optimality_gap_ < options_.grad_tolerance) { regularization_state_step_ = std::min(regularization_state_step_ / options_.regularization_state_factor, 1 / options_.regularization_state_factor); @@ -119,102 +151,43 @@ CDDPSolution CDDP::solveCLDDP() { // 2. Forward pass (either single-threaded or multi-threaded) ForwardPassResult best_result; best_result.cost = std::numeric_limits::infinity(); - bool forward_pass_feasible = false; + best_result.lagrangian = std::numeric_limits::infinity(); bool forward_pass_success = false; - if (!options_.use_parallel) { - // Single-threaded execution with early termination - for (double alpha : alphas_) { - ForwardPassResult result = solveCLDDPForwardPass(alpha); - - if (result.success && result.cost < best_result.cost) { - best_result = result; - forward_pass_feasible = true; - - // Check for early termination - double expected_cost_reduction = -alpha * (dV_(0) + 0.5 * alpha * dV_(1)); - if (expected_cost_reduction > 0.0) { - double cost_reduction_ratio = (J_ - result.cost) / expected_cost_reduction; - if (cost_reduction_ratio > options_.minimum_reduction_ratio) { - if (options_.debug) { - std::cout << "CDDP: Early termination due to successful forward pass" << std::endl; - } - break; - } - } - } - } - } else { - // TODO: Improve multi-threaded execution - // Multi-threaded execution - std::vector> futures; - futures.reserve(alphas_.size()); - - // Launch all forward passes in parallel - for (double alpha : alphas_) { - futures.push_back(std::async(std::launch::async, - [this, alpha]() { return solveCLDDPForwardPass(alpha); })); - } + // 2. Forward pass: line-search to find feasible optimal control sequence if backward pass is successful + for (double alpha : alphas_) { + ForwardPassResult result = solveASCDDPForwardPass(alpha); - // Collect results from all threads - for (auto& future : futures) { - try { - if (future.valid()) { - ForwardPassResult result = future.get(); - if (result.success && result.cost < best_result.cost) { - best_result = result; - forward_pass_feasible = true; - } - } - } catch (const std::exception& e) { - if (options_.verbose) { - std::cerr << "CDDP: Forward pass thread failed: " << e.what() << std::endl; + if (result.success && result.cost < best_result.cost) { + best_result = result; + forward_pass_success = true; + + // Check for early termination + if (result.success) { + if (options_.debug) { + std::cout << "CDDP: Early termination due to successful forward pass" << std::endl; } - continue; + break; } } } // Update solution if a feasible forward pass was found - if (forward_pass_feasible) { + if (forward_pass_success) { if (options_.debug) { std::cout << "Best cost: " << best_result.cost << std::endl; std::cout << "Best alpha: " << best_result.alpha << std::endl; } - - double expected_cost_reduction = -best_result.alpha * (dV_(0) + 0.5 * best_result.alpha * dV_(1)); - double cost_reduction_ratio; - - // Check if cost reduction is positive - if (expected_cost_reduction > 0.0) { - cost_reduction_ratio = (J_ - best_result.cost) / expected_cost_reduction; - } else { - cost_reduction_ratio = std::copysign(1.0, J_ - best_result.cost); - if (options_.debug) { - std::cerr << "CDDP: Expected cost reduction is non-positive" << std::endl; - } - } - - // Check if cost reduction is sufficient - if (cost_reduction_ratio > options_.minimum_reduction_ratio) { - forward_pass_success = true; - } else { - alpha_ = std::numeric_limits::infinity(); - if (options_.debug) { - std::cerr << "CDDP: Cost reduction ratio is too small" << std::endl; - } - } - } - - if (forward_pass_success) { X_ = best_result.state_sequence; U_ = best_result.control_sequence; dJ_ = J_ - best_result.cost; J_ = best_result.cost; + dL_ = L_ - best_result.lagrangian; + L_ = best_result.lagrangian; alpha_ = best_result.alpha; - // solution.lagrangian_sequence.push_back(best_result.lagrangian); solution.cost_sequence.push_back(J_); - + solution.lagrangian_sequence.push_back(L_); + // Decrease regularization regularization_state_step_ = std::min(regularization_state_step_ / options_.regularization_state_factor, 1 / options_.regularization_state_factor); regularization_state_ *= regularization_state_step_ * (regularization_state_ > options_.regularization_state_min ? 1.0 : 0.0); @@ -243,11 +216,10 @@ CDDPSolution CDDP::solveCLDDP() { // Print iteration information if (options_.verbose) { - printIteration(iter, J_, 0.0, optimality_gap_, regularization_state_, regularization_control_, alpha_); + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); } - - } + auto end_time = std::chrono::high_resolution_clock::now(); auto duration = std::chrono::duration_cast(end_time - start_time); @@ -255,25 +227,29 @@ CDDPSolution CDDP::solveCLDDP() { solution.state_sequence = X_; solution.control_sequence = U_; solution.alpha = alpha_; - solution.iterations = iter; solution.solve_time = duration.count(); // Time in microseconds - printSolution(solution); + if (options_.verbose) { + printSolution(solution); + } return solution; } -bool CDDP::solveCLDDPBackwardPass() { +// Backward pass +bool CDDP::solveASCDDPBackwardPass() +{ // Initialize variables const int state_dim = system_->getStateDim(); const int control_dim = system_->getControlDim(); + const auto active_set_tol = 1e-6; // Extract control box constraint auto control_box_constraint = getConstraint("ControlBoxConstraint"); - // Terminal cost and its derivatives - V_X_.back() = objective_->getFinalCostGradient(X_.back()); - V_XX_.back() = objective_->getFinalCostHessian(X_.back()); + // Terminal cost and its derivatives] + Eigen::VectorXd V_x = objective_->getFinalCostGradient(X_.back()); + Eigen::MatrixXd V_xx = objective_->getFinalCostHessian(X_.back()); // Pre-allocate matrices Eigen::MatrixXd A(state_dim, state_dim); @@ -288,35 +264,21 @@ bool CDDP::solveCLDDPBackwardPass() { Eigen::MatrixXd Q_uu_inv(control_dim, control_dim); Eigen::VectorXd k(control_dim); Eigen::MatrixXd K(control_dim, state_dim); - dV_ = Eigen::Vector2d::Zero(); - - // Create BoxQP solver - cddp::BoxQPOptions qp_options; - // qp_options.verbose = false; - // qp_options.maxIter = 1000; - // qp_options.minGrad = 1e-8; - // qp_options.minRelImprove = 1e-8; - // qp_options.minStep = 1e-22; - // qp_options.armijo = 0.1; - cddp::BoxQPSolver qp_solver(qp_options); - + Eigen::MatrixXd active_constraint_tabl(2 * (control_dim), horizon_); double Qu_error = 0.0; // Backward Riccati recursion - for (int t = horizon_ - 1; t >= 0; --t) { + for (int t = horizon_ - 1; t >= 0; --t) + { // Get state and control - const Eigen::VectorXd& x = X_[t]; - const Eigen::VectorXd& u = U_[t]; - - // Extract value function approximation - const Eigen::VectorXd& V_x = V_X_[t + 1]; - const Eigen::MatrixXd& V_xx = V_XX_[t + 1]; + const Eigen::VectorXd &x = X_[t]; + const Eigen::VectorXd &u = U_[t]; // Get continuous dynamics Jacobians const auto [Fx, Fu] = system_->getJacobians(x, u); // Convert continuous dynamics to discrete time - A = timestep_ * Fx; + A = timestep_ * Fx; A.diagonal().array() += 1.0; // More efficient way to add identity B = timestep_ * Fu; @@ -325,7 +287,23 @@ bool CDDP::solveCLDDPBackwardPass() { auto [l_x, l_u] = objective_->getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = objective_->getRunningCostHessians(x, u, t); - // Compute Q-function matrices + // TODO: Implement log barrier cost and its derivatives // FIXME: Implement log barrier cost and its derivatives + // Get log barrier cost and its derivatives + // for (const auto& constraint : constraint_set_) { + // if (constraint.first == "ControlBoxConstraint") { + // const double barrier_cost = getLogBarrierCost(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); + // l += barrier_cost; + // const auto [l_x_barrier, l_u_barrier] = getLogBarrierCostGradients(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); + // const auto [l_xx_barrier, l_uu_barrier, l_ux_barrier] = getLogBarrierCostHessians(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); + // l_x += l_x_barrier; + // l_u += l_u_barrier; + // // l_xx += l_xx_barrier; + // // l_uu += l_uu_barrier; + // // l_ux += l_ux_barrier; + // } + // } + + // Compute Q-function matrices Q_x = l_x + A.transpose() * V_x; Q_u = l_u + B.transpose() * V_x; Q_xx = l_xx + A.transpose() * V_xx * A; @@ -358,70 +336,100 @@ bool CDDP::solveCLDDPBackwardPass() { return false; } - if (control_box_constraint == nullptr) { + /* --- Identify Active Constraint --- */ + int active_constraint_index = 0; + Eigen::MatrixXd C(control_dim, control_dim); // Control constraint matrix + Eigen::MatrixXd D(control_dim, state_dim); // State constraint matrix + + // TODO: Implement active set method + for (int j = 0; j < control_dim; j++) + { + if (u(j) <= control_box_constraint->getLowerBound()(j) + active_set_tol) + { + Eigen::VectorXd e = Eigen::VectorXd::Zero(control_dim); + e(j) = 1.0; + C.row(active_constraint_index) = -e; // Note the negative sign + D.row(active_constraint_index) = Eigen::VectorXd::Zero(state_dim); + active_constraint_index += 1; + } + else if (u(j) >= control_box_constraint->getUpperBound()(j) - active_set_tol) + { + Eigen::VectorXd e = Eigen::VectorXd::Zero(control_dim); + e(j) = 1.0; // No negative here + C.row(active_constraint_index) = e; + D.row(active_constraint_index) = Eigen::VectorXd::Zero(state_dim); + active_constraint_index += 1; + } + } + + Eigen::MatrixXd active_constraint_table = Eigen::MatrixXd::Zero(2 * (control_dim), horizon_); + if (active_constraint_index == 0) + { // No active constraints const Eigen::MatrixXd &H = Q_uu_reg.inverse(); k = -H * Q_u; K = -H * Q_ux_reg; - if (options_.debug) { - std::cout << "No control box constraint" << std::endl; - } - } else { - // Solve QP by boxQP - const Eigen::VectorXd& lb = control_box_constraint->getLowerBound() - u; - const Eigen::VectorXd& ub = control_box_constraint->getUpperBound() - u; - const Eigen::VectorXd& x0 = k_[t]; // Initial guess - - cddp::BoxQPResult qp_result = qp_solver.solve(Q_uu_reg, Q_u, lb, ub, x0); - - if (qp_result.status == BoxQPStatus::HESSIAN_NOT_PD || - qp_result.status == BoxQPStatus::NO_DESCENT) { - if (options_.debug) { - std::cerr << "CDDP: BoxQP failed at time step " << t << std::endl; - } - return false; - } - - // Extract solution - k = qp_result.x; - - // Compute feedback gain matrix - K = Eigen::MatrixXd::Zero(control_dim, state_dim); - if (qp_result.free.sum() > 0) { - // Get indices of free variables - std::vector free_idx; - for (int i = 0; i < control_dim; i++) { - if (qp_result.free(i)) { - free_idx.push_back(i); - } + } + else + { + // Extract identified active constraints + Eigen::MatrixXd grad_x_g = D.topRows(active_constraint_index); + Eigen::MatrixXd grad_u_g = C.topRows(active_constraint_index); + + // Calculate Lagrange multipliers + Eigen::MatrixXd Q_uu_inv = Q_uu_reg.inverse(); + Eigen::MatrixXd lambda = -(grad_u_g * Q_uu_inv * grad_u_g.transpose()).inverse() * (grad_u_g * Q_uu_inv * Q_u); + + // Find indices where lambda is non-negative + std::vector active_indices; + for (int i = 0; i < lambda.rows(); ++i) + { + if (lambda(i) >= 0) + { + active_indices.push_back(i); } - - // Extract relevant parts of Q_ux for free variables - Eigen::MatrixXd Q_ux_free(free_idx.size(), state_dim); - for (size_t i = 0; i < free_idx.size(); i++) { - Q_ux_free.row(i) = Q_ux_reg.row(free_idx[i]); + } + int active_count_new = active_indices.size(); + + // Create new constraint matrices + Eigen::MatrixXd C_new = Eigen::MatrixXd::Zero(active_count_new, control_dim); + Eigen::MatrixXd D_new = Eigen::MatrixXd::Zero(active_count_new, state_dim); + + if (active_count_new > 0) + { + // Fill new constraint matrices with active constraints + for (int i = 0; i < active_count_new; ++i) + { + C_new.row(i) = grad_u_g.row(active_indices[i]); + D_new.row(i) = grad_x_g.row(active_indices[i]); } - // Compute gains for free variables using the LDLT factorization - Eigen::MatrixXd K_free = -qp_result.Hfree.solve(Q_ux_free); - - // Put back into full K matrix - for (size_t i = 0; i < free_idx.size(); i++) { - K.row(free_idx[i]) = K_free.row(i); - } + // Calculate feedback gains + Eigen::MatrixXd W = -(C_new * Q_uu_inv * C_new.transpose()).inverse() * (C_new * Q_uu_inv); + Eigen::MatrixXd H = Q_uu_inv * (Eigen::MatrixXd::Identity(control_dim, control_dim) - C_new.transpose() * W); + k = -H * Q_u; + K = -H * Q_ux_reg + W.transpose() * D_new; + } + else + { + // If no active constraints remain, revert to unconstrained solution + Eigen::MatrixXd H = Q_uu_reg.inverse(); + K = -H * Q_ux_reg; + k = -H * Q_u; } } - // Store feedforward and feedback gain - k_[t] = k; - K_[t] = K; + // Store Q-function matrices + Q_UU_[t] = Q_uu_reg; + Q_UX_[t] = Q_ux_reg; + Q_U_[t] = Q_u; // Compute value function approximation Eigen::Vector2d dV_step; dV_step << Q_u.dot(k), 0.5 * k.dot(Q_uu * k); dV_ = dV_ + dV_step; - V_X_[t] = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; - V_XX_[t] = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; - V_XX_[t] = 0.5 * (V_XX_[t] + V_XX_[t].transpose()); // Symmetrize Hessian + V_x = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; + V_xx = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; + V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize Hessian // Compute optimality gap (Inf-norm) for convergence check Qu_error = std::max(Qu_error, Q_u.lpNorm()); @@ -430,41 +438,85 @@ bool CDDP::solveCLDDPBackwardPass() { optimality_gap_ = Qu_error; } - expected_ = dV_(0); - - if (options_.debug) { - std::cout << "Qu_error: " << Qu_error << std::endl; - std::cout << "dV: " << dV_.transpose() << std::endl; - } - return true; } -ForwardPassResult CDDP::solveCLDDPForwardPass(double alpha) { +// Forward pass +ForwardPassResult CDDP::solveASCDDPForwardPass(double alpha) +{ + // Prepare result struct ForwardPassResult result; + result.success = false; + result.cost = std::numeric_limits::infinity(); + result.lagrangian = std::numeric_limits::infinity(); + result.alpha = alpha; + const int state_dim = system_->getStateDim(); const int control_dim = system_->getControlDim(); + // Extract control box constraint + auto control_box_constraint = getConstraint("ControlBoxConstraint"); + // Initialize trajectories std::vector X_new = X_; std::vector U_new = U_; X_new[0] = initial_state_; double J_new = 0.0; - auto control_box_constraint = getConstraint("ControlBoxConstraint"); - for (int t = 0; t < horizon_; ++t) { const Eigen::VectorXd& x = X_new[t]; const Eigen::VectorXd& u = U_new[t]; const Eigen::VectorXd& delta_x = x - X_[t]; - U_new[t] = u + alpha * k_[t] + K_[t] * delta_x; + // Extract Q-function matrices + const Eigen::VectorXd &Q_u = Q_U_[t]; + const Eigen::MatrixXd &Q_uu = Q_UU_[t]; + const Eigen::MatrixXd &Q_ux = Q_UX_[t]; + + const Eigen::VectorXd &q = alpha * Q_u + Q_ux * delta_x; // Gradient of QP objective + + // Lower and upper bounds + const Eigen::VectorXd& lb = control_box_constraint->getLowerBound() - u; + const Eigen::VectorXd& ub = control_box_constraint->getUpperBound() - u; + + // Create b vector + Eigen::VectorXd b(2 * control_dim); + b << ub, -lb; + + // Create A matrix + Eigen::MatrixXd A(2 * control_dim, control_dim); // Upper is identity, lower is -identity + A.setIdentity(); + A.block(control_dim, 0, control_dim, control_dim) = -Eigen::MatrixXd::Identity(control_dim, control_dim); + + QPSolverOptions options; + options.verbose = false; + QPSolver solver(options); - if (control_box_constraint != nullptr) { - U_new[t] = control_box_constraint->clamp(U_new[t]); + solver.setDimensions(state_dim, 2*control_dim); + solver.setHessian(Q_uu); + solver.setGradient(q); + solver.setConstraints(A, b); + + QPResult qp_result = solver.solve(); + + if (qp_result.status != QPStatus::OPTIMAL) { + if (options_.debug) { + std::cerr << "CDDP: QP solver failed" << std::endl; + } + result.success = false; + break; } + // Extract solution + const Eigen::VectorXd &delta_u = qp_result.x; + + // Extract solution + U_new[t] += delta_u; + + // Compute cost J_new += objective_->running_cost(x, U_new[t], t); + + // Compute new state X_new[t + 1] = system_->getDiscreteDynamics(x, U_new[t]); } J_new += objective_->terminal_cost(X_new.back()); @@ -473,14 +525,12 @@ ForwardPassResult CDDP::solveCLDDPForwardPass(double alpha) { double reduction_ratio = expected > 0.0 ? dJ / expected : std::copysign(1.0, dJ); // Check if cost reduction is sufficient - result.success = reduction_ratio > options_.minimum_reduction_ratio; + result.success = true; result.state_sequence = X_new; result.control_sequence = U_new; result.cost = J_new; - result.alpha = alpha; + result.lagrangian = J_new; return result; } - - } // namespace cddp \ No newline at end of file diff --git a/src/cddp_core/boxqp.cpp b/src/cddp_core/boxqp.cpp index a6e46dc..a734ce0 100644 --- a/src/cddp_core/boxqp.cpp +++ b/src/cddp_core/boxqp.cpp @@ -43,12 +43,12 @@ BoxQPResult BoxQPSolver::solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& double old_value = std::numeric_limits::infinity(); // Main iteration loop - for (int iter = 0; iter < options_.maxIter; ++iter) { + for (int iter = 0; iter < options_.max_iterations; ++iter) { result.iterations = iter + 1; // Check relative improvement - if (iter > 0 && std::abs(old_value - value) < options_.minRelImprove * std::abs(old_value)) { - result.status = BoxQPStatus::SMALL_IMPROVEMENT; + if (iter > 0 && std::abs(old_value - value) < options_.min_rel_improve * std::abs(old_value)) { + result.status = BoxQPStatus::SUCCESS; break; } old_value = value; @@ -113,8 +113,8 @@ BoxQPResult BoxQPSolver::solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& grad_norm = std::sqrt(grad_norm); result.final_grad_norm = grad_norm; - if (grad_norm < options_.minGrad) { - result.status = BoxQPStatus::SMALL_GRADIENT; + if (grad_norm < options_.min_grad) { + result.status = BoxQPStatus::SUCCESS; break; } @@ -210,7 +210,7 @@ std::pair> BoxQPSolver::lineSearch( double step = 1.0; const double sdotg = search.dot(gradient); - while (step > options_.minStep) { + while (step > options_.min_step) { // Compute candidate Eigen::VectorXd x_new = projectOntoBox(x + step * search, lower, upper); @@ -222,7 +222,7 @@ std::pair> BoxQPSolver::lineSearch( return {true, {step, x_new}}; } - step *= options_.stepDec; + step *= options_.step_dec; } return {false, {0.0, x}}; diff --git a/src/cddp_core/cddp_core.cpp b/src/cddp_core/cddp_core.cpp index 373b119..6d371dd 100644 --- a/src/cddp_core/cddp_core.cpp +++ b/src/cddp_core/cddp_core.cpp @@ -24,7 +24,7 @@ #include // For parallel execution policies #include "cddp_core/cddp_core.hpp" -#include "cddp_core/qp_solver.hpp" +#include "cddp_core/boxqp.hpp" namespace cddp { @@ -33,37 +33,84 @@ namespace cddp CDDP::CDDP(const Eigen::VectorXd &initial_state, const Eigen::VectorXd &reference_state, int horizon, - double timestep) + double timestep, + std::unique_ptr system, + std::unique_ptr objective, + const CDDPOptions &options) : initial_state_(initial_state), reference_state_(reference_state), horizon_(horizon), - timestep_(timestep) + timestep_(timestep), + system_(std::move(system)), + objective_(std::move(objective)), + options_(options), + initialized_(false) { + initializeCDDP(); + if (options_.verbose) { + printSolverInfo(); + printOptions(options_); + } +} + +void CDDP::setInitialTrajectory(const std::vector &X, const std::vector &U) +{ + if (!system_) { + std::cerr << "CDDP::setInitialTrajectory: No dynamical system provided." << std::endl; + throw std::runtime_error("CDDP::setInitialTrajectory: No dynamical system provided."); + } + + if (!objective_) { + std::cerr << "CDDP::setInitialTrajectory: No objective function provided." << std::endl; + throw std::runtime_error("CDDP::setInitialTrajectory: No objective function provided."); + } - printSolverInfo(); + if (X.size() != horizon_ + 1) + { + std::cerr << "CDDP::setInitialTrajectory: X has wrong #timesteps" << std::endl; + throw std::runtime_error("CDDP::setInitialTrajectory: X has wrong #timesteps"); + } + if (U.size() != horizon_) + { + std::cerr << "CDDP::setInitialTrajectory: U has wrong #timesteps" << std::endl; + throw std::runtime_error("CDDP::setInitialTrajectory: U has wrong #timesteps"); + } - // initializeCDDP(); + X_ = X; + U_ = U; + J_ = objective_->evaluate(X_, U_); } // Initialize the CDDP solver void CDDP::initializeCDDP() { - const int state_dim = system_->getStateDim(); - const int control_dim = system_->getControlDim(); + if (initialized_) + { + // Already done—return. + return; + } - // Check if the system and objective are set if (!system_) { - std::cerr << "CDDP: Dynamical system is not set" << std::endl; - throw std::runtime_error("Dynamical system is not set"); + initialized_ = false; + if (options_.verbose) { + std::cerr << "CDDP::initializeCDDP: No dynamical system provided." << std::endl; + } + return; } if (!objective_) { - std::cerr << "CDDP: Objective function is not set" << std::endl; - throw std::runtime_error("Objective function is not set"); + initialized_ = false; + if (options_.verbose) { + std::cerr << "CDDP::initializeCDDP: No objective function provided." << std::endl; + } + return; } + const int state_dim = system_->getStateDim(); + const int control_dim = system_->getControlDim(); + // Check if reference_state in objective and reference_state in CDDP are the same if ((reference_state_ - objective_->getReferenceState()).norm() > 1e-6) { @@ -87,7 +134,7 @@ void CDDP::initializeCDDP() } // Initialize cost - J_ = 0.0; + J_ = objective_->evaluate(X_, U_); alpha_ = options_.backtracking_coeff; for (int i = 0; i < options_.max_line_search_iterations; ++i) @@ -116,187 +163,70 @@ void CDDP::initializeCDDP() } - // Initialize gains and value function approximation + // Initialize gains and value reduction k_.resize(horizon_, Eigen::VectorXd::Zero(control_dim)); K_.resize(horizon_, Eigen::MatrixXd::Zero(control_dim, state_dim)); dV_.resize(2); - V_X_.resize(horizon_ + 1, Eigen::VectorXd::Zero(state_dim)); - V_XX_.resize(horizon_ + 1, Eigen::MatrixXd::Zero(state_dim, state_dim)); - // Initialize Q-function matrices + // Initialize Q-function matrices: USED ONLY FOR ASCDDP Q_UU_.resize(horizon_, Eigen::MatrixXd::Zero(control_dim, control_dim)); Q_UX_.resize(horizon_, Eigen::MatrixXd::Zero(control_dim, state_dim)); Q_U_.resize(horizon_, Eigen::VectorXd::Zero(control_dim)); - // Initialize constraints if empty - if (constraint_set_.empty()) - { - std::cerr << "CDDP: No constraints are set" << std::endl; - } // if control constraints are set - else if (constraint_set_.find("ControlBoxConstraint") != constraint_set_.end()) + // Check if ControlBoxConstraint is set + if (constraint_set_.find("ControlBoxConstraint") != constraint_set_.end()) { std::cout << "ControlBoxConstraint is set" << std::endl; } - // Initialize Log-barrier (std::unique_ptr log_barrier_;) + // Initialize Log-barrier object log_barrier_ = std::make_unique(options_.barrier_coeff, options_.relaxation_coeff, options_.barrier_order); - // // Initialize OSQP setting - // osqp::OsqpSettings settings; - // // settings.warm_start = true; - // settings.verbose = false; - // settings.max_iter = 1000; - // settings.eps_abs = 1e-3; - // settings.eps_rel = 1e-2; - // // settings.eps_prim_inf = 1e-4; - // // settings.eps_dual_inf = 1e-4; - // // settings.alpha = 1.6; - - // // Initialize QP solver instance - // osqp::OsqpInstance instance; - - // Eigen::SparseMatrix P(control_dim, control_dim); - // P.setIdentity(); - // P.makeCompressed(); - // instance.objective_matrix = P; - - // instance.objective_vector = Eigen::VectorXd::Zero(control_dim); - - // Eigen::SparseMatrix A(control_dim, control_dim); - // A.setIdentity(); - // A.makeCompressed(); - // instance.constraint_matrix = A; - - // instance.lower_bounds = Eigen::VectorXd::Constant(control_dim, -std::numeric_limits::infinity()); - // instance.upper_bounds = Eigen::VectorXd::Constant(control_dim, std::numeric_limits::infinity()); - - // // Initialize the solver - // osqp_solver_.Init(instance, settings); - - // // Ceck if the problem is initialized correctly - // if (osqp_solver_.IsInitialized()) - // { - // // std::cout << "OSQP Solver is initialized" << std::endl; - // } - // else - // { - // std::cerr << "OSQP Solver is not initialized" << std::endl; - // } - - // for (int t = 0; t < horizon_ + 1; ++t) - // { - // X_[t] = initial_state_; - // } - - // // Initialize Trajectory by control-limited forward pass - // bool is_feasible = false; - - // // Initialize trajectory by forward pass - // auto control_box_constraint = getConstraint("ControlBoxConstraint"); - // // Line-search iteration - // double alpha = options_.backtracking_coeff; - // for (int iter = 0; iter < options_.max_line_search_iterations; ++iter) { - // // Initialize cost and constraints - // double J_new = 0.0; - // double L_new = 0.0; - // bool is_feasible = true; - - // std::vector X_new = X_; - // std::vector U_new = U_; - // X_new[0] = initial_state_; - - // for (int t = 0; t < horizon_; ++t) - // { - // const Eigen::VectorXd &x = X_new[t]; - // const Eigen::VectorXd &u = U_new[t]; - - // Eigen::VectorXd dx = x - X_[t]; - // Eigen::VectorXd du = alpha * k_[t] + K_[t] * dx; - // U_new[t] = u + du; - - // // Clamp control input - // if (control_box_constraint != nullptr) { - // U_new[t] = control_box_constraint->clamp(U_new[t]); - // } - - // const double step_cost = objective_->running_cost(x, U_new[t], t); - // const double barrier_cost = log_barrier_->evaluate(*control_box_constraint, x, u); - - // J_new += step_cost; - // L_new += (step_cost + barrier_cost); - - // X_new[t + 1] = system_->getDiscreteDynamics(x, U_new[t]); - // } - - // J_new += objective_->terminal_cost(X_new.back()); - // L_new += objective_->terminal_cost(X_new.back()); - - // double dJ = J_ - J_new; - // double dL = L_ - L_new; - - // // If state is not diverged, break the loop - // if (X_[horizon_].norm() < 1e+8) { - // is_feasible = true; - // break; - // } - // alpha *= options_.backtracking_coeff; - // } + // Initialize boxqp options + boxqp_options_.max_iterations = options_.boxqp_max_iterations; + boxqp_options_.min_grad = options_.boxqp_min_grad; + boxqp_options_.min_rel_improve = options_.boxqp_min_rel_improve; + boxqp_options_.step_dec = options_.boxqp_step_dec; + boxqp_options_.min_step = options_.boxqp_min_step; + boxqp_options_.armijo = options_.boxqp_armijo; + boxqp_options_.verbose = options_.boxqp_verbose; + + boxqp_solver_ = BoxQPSolver(boxqp_options_); - // if (!is_feasible) { - // std::cerr << "CDDP: Initial trajectory is not feasible" << std::endl; - // throw std::runtime_error("Initial trajectory is not feasible"); - // } + initialized_ = true; } -// Solve the problem -CDDPSolution CDDP::solve() -{ - // Initialize CDDP solver - initializeCDDP(); - if (options_.verbose) - { - printOptions(options_); +CDDPSolution CDDP::solve() { + // Initialize if not done + if (!initialized_) { + initializeCDDP(); + } + + if (!initialized_) { + std::cerr << "CDDP: Initialization failed" << std::endl; + throw std::runtime_error("CDDP: Initialization failed"); } - // Initialize solution + // Prepare solution struct CDDPSolution solution; solution.converged = false; + solution.alpha = alpha_; + solution.iterations = 0; + solution.solve_time = 0.0; solution.time_sequence.reserve(horizon_ + 1); - for (int t = 0; t <= horizon_; ++t) - { + for (int t = 0; t <= horizon_; ++t) { solution.time_sequence.push_back(timestep_ * t); } - solution.cost_sequence.reserve(options_.max_iterations); // Reserve space for efficiency + solution.control_sequence.reserve(horizon_); + solution.state_sequence.reserve(horizon_ + 1); + solution.cost_sequence.reserve(options_.max_iterations); // Reserve space for efficiency solution.lagrangian_sequence.reserve(options_.max_iterations); // Reserve space for efficiency - - // Evaluate initial cost - J_ = objective_->evaluate(X_, U_); solution.cost_sequence.push_back(J_); + solution.lagrangian_sequence.push_back(L_); - // Evaluate Lagrangian - L_ = J_; - - // // Loop over horizon - // for (int t = 0; t < 1; ++t) { - // // Evaluate state constraint violation - // for (const auto& constraint : constraint_set_) { - // if (constraint.first == "ControlBoxConstraint") { - // L_ += getLogBarrierCost(*constraint.second, X_[t], U_[t], barrier_coeff_, options_.relaxation_coeff); - // // Eigen::VectorXd constraint_violation = constraint.second->evaluate(X_[t], U_[t]); - // // if (constraint_violation.minCoeff() < 0) { - // // std::cerr << "CDDP: Constraint violation at time " << t << std::endl; - // // std::cerr << "Constraint violation: " << constraint_violation.transpose() << std::endl; - // // throw std::runtime_error("Constraint violation"); - // // } - // } - - // } - // } - - if (options_.verbose) - { - printIteration(0, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information + if (options_.verbose) { + printIteration(0, J_, J_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information } // Start timer @@ -307,27 +237,26 @@ CDDPSolution CDDP::solve() while (iter < options_.max_iterations) { ++iter; + solution.iterations = iter; // Check maximum CPU time - if (options_.max_cpu_time > 0) - { + if (options_.max_cpu_time > 0) { auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_time - start_time); - if (duration.count() * 1e-6 > options_.max_cpu_time) - { - std::cerr << "CDDP: Maximum CPU time reached. Returning current solution" << std::endl; + auto duration = std::chrono::duration_cast(end_time - start_time); + if (duration.count() * 1e-6 > options_.max_cpu_time) { + if (options_.verbose) { + std::cerr << "CDDP: Maximum CPU time reached. Returning current solution" << std::endl; + } break; } } // 1. Backward pass: Solve Riccati recursion to compute optimal control law bool backward_pass_success = false; - while (!backward_pass_success) - { + while (!backward_pass_success) { backward_pass_success = solveBackwardPass(); - if (!backward_pass_success) - { + if (!backward_pass_success) { std::cerr << "CDDP: Backward pass failed" << std::endl; // Increase regularization @@ -336,103 +265,159 @@ CDDPSolution CDDP::solve() regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); regularization_control_ = std::max(regularization_control_ * regularization_control_step_, options_.regularization_control_min); - if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) - { - std::cerr << "CDDP: Regularization limit reached" << std::endl; + if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) { + if (options_.verbose) { + std::cerr << "CDDP: Regularization limit reached" << std::endl; + } break; // Exit if regularization limit reached } continue; // Continue if backward pass fails } } - + // Check termination due to small cost improvement - if (optimality_gap_ < options_.grad_tolerance && regularization_state_ < 1e-4 && regularization_control_ < 1e-4) - { + if (optimality_gap_ < options_.grad_tolerance) { regularization_state_step_ = std::min(regularization_state_step_ / options_.regularization_state_factor, 1 / options_.regularization_state_factor); - regularization_state_ *= regularization_state_step_; - if (regularization_state_ <= options_.regularization_state_min) - { - regularization_state_ = 0.0; - } + regularization_state_ *= regularization_state_step_ * (regularization_state_ > options_.regularization_state_min ? 1.0 : 0.0); regularization_control_step_ = std::min(regularization_control_step_ / options_.regularization_control_factor, 1 / options_.regularization_control_factor); - regularization_control_ *= regularization_control_step_; - if (regularization_control_ <= options_.regularization_control_min) - { - regularization_control_ = 0.0; + regularization_control_ *= regularization_control_step_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); + + if (regularization_state_ < 1e-5 && regularization_control_ < 1e-5) { + solution.converged = true; + break; } - - solution.converged = true; - break; + } + // 2. Forward pass (either single-threaded or multi-threaded) + ForwardPassResult best_result; + best_result.cost = std::numeric_limits::infinity(); + best_result.lagrangian = std::numeric_limits::infinity(); bool forward_pass_success = false; - // 2. Forward pass: line-search to find feasible optimal control sequence if backward pass is successful - if (backward_pass_success) - { - forward_pass_success = solveForwardPass(); - } - // Print iteration information - if (options_.verbose) - { - printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); + if (!options_.use_parallel) { + // Single-threaded execution with early termination + for (double alpha : alphas_) { + ForwardPassResult result = solveForwardPass(alpha); + + if (result.success && result.cost < best_result.cost) { + best_result = result; + forward_pass_success = true; + + // Check for early termination + if (result.success) { + if (options_.debug) { + std::cout << "CDDP: Early termination due to successful forward pass" << std::endl; + } + break; + } + } + } + } else { + // TODO: Improve multi-threaded execution + // Multi-threaded execution + std::vector> futures; + futures.reserve(alphas_.size()); + + // Launch all forward passes in parallel + for (double alpha : alphas_) { + futures.push_back(std::async(std::launch::async, + [this, alpha]() { return solveForwardPass(alpha); })); + } + + // Collect results from all threads + for (auto& future : futures) { + try { + if (future.valid()) { + ForwardPassResult result = future.get(); + if (result.success && result.cost < best_result.cost) { + best_result = result; + forward_pass_success = true; + } + } + } catch (const std::exception& e) { + if (options_.verbose) { + std::cerr << "CDDP: Forward pass thread failed: " << e.what() << std::endl; + } + continue; + } + } } - if (forward_pass_success) - { + // Update solution if a feasible forward pass was found + if (forward_pass_success) { + if (options_.debug) { + std::cout << "Best cost: " << best_result.cost << std::endl; + std::cout << "Best alpha: " << best_result.alpha << std::endl; + } + X_ = best_result.state_sequence; + U_ = best_result.control_sequence; + dJ_ = J_ - best_result.cost; + J_ = best_result.cost; + dL_ = L_ - best_result.lagrangian; + L_ = best_result.lagrangian; + alpha_ = best_result.alpha; + solution.cost_sequence.push_back(J_); + solution.lagrangian_sequence.push_back(L_); + // Decrease regularization regularization_state_step_ = std::min(regularization_state_step_ / options_.regularization_state_factor, 1 / options_.regularization_state_factor); - regularization_state_ *= regularization_state_step_; - if (regularization_state_ <= options_.regularization_state_min) - { - regularization_state_ = 0.0; - } + regularization_state_ *= regularization_state_step_ * (regularization_state_ > options_.regularization_state_min ? 1.0 : 0.0); regularization_control_step_ = std::min(regularization_control_step_ / options_.regularization_control_factor, 1 / options_.regularization_control_factor); - regularization_control_ *= regularization_control_step_; - if (regularization_control_ <= options_.regularization_control_min) - { - regularization_control_ = 0.0; - } + regularization_control_ *= regularization_control_step_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); - // Append Latest Cost - solution.cost_sequence.push_back(J_); - - if (dJ_ < options_.cost_tolerance) - { + // Check termination + if (dJ_ < options_.cost_tolerance) { solution.converged = true; - solution.iterations = iter; + break; + } + } else { + // Increase regularization + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_state_ = std::max(regularization_state_ * regularization_state_step_, options_.regularization_state_max); + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + regularization_control_ = std::max(regularization_control_ * regularization_control_step_, options_.regularization_control_max); + + // Check regularization limit + if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) { + std::cerr << "CDDP: Regularization limit reached" << std::endl; + solution.converged = false; break; } } + + // Print iteration information + if (options_.verbose) { + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); + } } + auto end_time = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end_time - start_time); // Finalize solution - solution.control_sequence = U_; solution.state_sequence = X_; - solution.iterations = solution.converged ? iter : options_.max_iterations; - - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_time - start_time); + solution.control_sequence = U_; + solution.alpha = alpha_; solution.solve_time = duration.count(); // Time in microseconds - printSolution(solution); + + if (options_.verbose) { + printSolution(solution); + } return solution; } -// Backward pass -bool CDDP::solveBackwardPass() -{ +bool CDDP::solveBackwardPass() { // Initialize variables const int state_dim = system_->getStateDim(); const int control_dim = system_->getControlDim(); - const auto active_set_tol = options_.active_set_tolerance; // Extract control box constraint auto control_box_constraint = getConstraint("ControlBoxConstraint"); - // Terminal cost and its derivatives - V_X_.back() = objective_->getFinalCostGradient(X_.back()); - V_XX_.back() = objective_->getFinalCostHessian(X_.back()); + // Terminal cost and its derivatives] + Eigen::VectorXd V_x = objective_->getFinalCostGradient(X_.back()); + Eigen::MatrixXd V_xx = objective_->getFinalCostHessian(X_.back()); // Pre-allocate matrices Eigen::MatrixXd A(state_dim, state_dim); @@ -441,31 +426,28 @@ bool CDDP::solveBackwardPass() Eigen::VectorXd Q_u(control_dim); Eigen::MatrixXd Q_xx(state_dim, state_dim); Eigen::MatrixXd Q_uu(control_dim, control_dim); + Eigen::MatrixXd Q_uu_reg(control_dim, control_dim); Eigen::MatrixXd Q_ux(control_dim, state_dim); + Eigen::MatrixXd Q_ux_reg(control_dim, state_dim); Eigen::MatrixXd Q_uu_inv(control_dim, control_dim); - Eigen::MatrixXd active_constraint_tabl(2 * (control_dim), horizon_); Eigen::VectorXd k(control_dim); Eigen::MatrixXd K(control_dim, state_dim); + dV_ = Eigen::Vector2d::Zero(); double Qu_error = 0.0; // Backward Riccati recursion - for (int t = horizon_ - 1; t >= 0; --t) - { + for (int t = horizon_ - 1; t >= 0; --t) { // Get state and control - const Eigen::VectorXd &x = X_[t]; - const Eigen::VectorXd &u = U_[t]; - - // Extract value function approximation - const Eigen::VectorXd &V_x = V_X_[t + 1]; - const Eigen::MatrixXd &V_xx = V_XX_[t + 1]; + const Eigen::VectorXd& x = X_[t]; + const Eigen::VectorXd& u = U_[t]; // Get continuous dynamics Jacobians const auto [Fx, Fu] = system_->getJacobians(x, u); // Convert continuous dynamics to discrete time - A = timestep_ * Fx; - A.diagonal().array() += 1.0; // More efficient way to add identity + A = timestep_ * Fx; + A.diagonal().array() += 1.0; B = timestep_ * Fu; // Get cost and its derivatives @@ -473,168 +455,101 @@ bool CDDP::solveBackwardPass() auto [l_x, l_u] = objective_->getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = objective_->getRunningCostHessians(x, u, t); - // TODO: Implement log barrier cost and its derivatives // FIXME: Implement log barrier cost and its derivatives - // Get log barrier cost and its derivatives - // for (const auto& constraint : constraint_set_) { - // if (constraint.first == "ControlBoxConstraint") { - // const double barrier_cost = getLogBarrierCost(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); - // l += barrier_cost; - // const auto [l_x_barrier, l_u_barrier] = getLogBarrierCostGradients(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); - // const auto [l_xx_barrier, l_uu_barrier, l_ux_barrier] = getLogBarrierCostHessians(*constraint.second, x, u, barrier_coeff_, options_.relaxation_coeff); - // l_x += l_x_barrier; - // l_u += l_u_barrier; - // // l_xx += l_xx_barrier; - // // l_uu += l_uu_barrier; - // // l_ux += l_ux_barrier; - // } - // } - - // Compute Q-function matrices + // Compute Q-function matrices Q_x = l_x + A.transpose() * V_x; Q_u = l_u + B.transpose() * V_x; Q_xx = l_xx + A.transpose() * V_xx * A; - - if (options_.regularization_type == "state" || options_.regularization_type == "both") - { - Q_ux = l_ux + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * A; - Q_uu = l_uu + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * B; - } - else - { - Q_ux = l_ux + B.transpose() * V_xx * A; - Q_uu = l_uu + B.transpose() * V_xx * B; - } - - // Symmetrize Q_uu for cholensky decomposition - Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); - - // Control Regularization - if (options_.regularization_type == "control" || options_.regularization_type == "both") - { - Q_uu += options_.regularization_control * Eigen::MatrixXd::Identity(control_dim, control_dim); + Q_ux = l_ux + B.transpose() * V_xx * A; + Q_uu = l_uu + B.transpose() * V_xx * B; + + // TODO: Apply Cholesky decomposition to Q_uu later? + // // Symmetrize Q_uu for Cholesky decomposition + // Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); + + if (options_.regularization_type == "state" || options_.regularization_type == "both") { + Q_ux_reg = l_ux + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * A; + Q_uu_reg = l_uu + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * B; + } else { + Q_ux_reg = Q_ux; + Q_uu_reg = Q_uu; + } + + if (options_.regularization_type == "control" || options_.regularization_type == "both") { + Q_uu_reg.diagonal().array() += regularization_control_; } // Check eigenvalues of Q_uu - Eigen::EigenSolver es(Q_uu); - Eigen::VectorXd eigenvalues = es.eigenvalues().real(); - if (eigenvalues.minCoeff() <= 0) - { - // Add regularization - // Q_uu.diagonal() += 1e-6; - std::cout << "Q_uu is not positive definite at " << t << std::endl; - - eigenvalues = es.eigenvalues().real(); - if (eigenvalues.minCoeff() <= 0) - { - std::cout << "Q_uu is still not positive definite" << std::endl; - return false; + Eigen::EigenSolver es(Q_uu_reg); + const Eigen::VectorXd& eigenvalues = es.eigenvalues().real(); + if (eigenvalues.minCoeff() <= 0) { + if (options_.debug) { + std::cerr << "CDDP: Q_uu is still not positive definite" << std::endl; } - } - - // Cholesky decomposition - Eigen::LLT llt(Q_uu); - if (llt.info() != Eigen::Success) - { - // Decomposition failed - std::cout << "Cholesky decomposition failed" << std::endl; return false; } - /* --- Identify Active Constraint --- */ - int active_constraint_index = 0; - Eigen::MatrixXd C(control_dim, control_dim); // Control constraint matrix - Eigen::MatrixXd D(control_dim, state_dim); // State constraint matrix - - // TODO: Implement active set method - for (int j = 0; j < control_dim; j++) - { - if (u(j) <= control_box_constraint->getLowerBound()(j) + active_set_tol) - { - Eigen::VectorXd e = Eigen::VectorXd::Zero(control_dim); - e(j) = 1.0; - C.row(active_constraint_index) = -e; // Note the negative sign - D.row(active_constraint_index) = Eigen::VectorXd::Zero(state_dim); - active_constraint_index += 1; - } - else if (u(j) >= control_box_constraint->getUpperBound()(j) - active_set_tol) - { - Eigen::VectorXd e = Eigen::VectorXd::Zero(control_dim); - e(j) = 1.0; // No negative here - C.row(active_constraint_index) = e; - D.row(active_constraint_index) = Eigen::VectorXd::Zero(state_dim); - active_constraint_index += 1; - } - } - - Eigen::MatrixXd active_constraint_table = Eigen::MatrixXd::Zero(2 * (control_dim), horizon_); - if (active_constraint_index == 0) - { // No active constraints - // Compute the optimal control deviation - Eigen::MatrixXd H = Q_uu.inverse(); - K = -H * Q_ux; + if (control_box_constraint == nullptr) { + const Eigen::MatrixXd &H = Q_uu_reg.inverse(); k = -H * Q_u; - } - else - { - // Extract identified active constraints - Eigen::MatrixXd grad_x_g = D.topRows(active_constraint_index); - Eigen::MatrixXd grad_u_g = C.topRows(active_constraint_index); - - // Calculate Lagrange multipliers - Eigen::MatrixXd Q_uu_inv = Q_uu.inverse(); - Eigen::MatrixXd lambda = -(grad_u_g * Q_uu_inv * grad_u_g.transpose()).inverse() * (grad_u_g * Q_uu_inv * Q_u); - - // Find indices where lambda is non-negative - std::vector active_indices; - for (int i = 0; i < lambda.rows(); ++i) - { - if (lambda(i) >= 0) - { - active_indices.push_back(i); - } + K = -H * Q_ux_reg; + } else { + // Solve QP by boxQP + const Eigen::VectorXd& lb = control_box_constraint->getLowerBound() - u; + const Eigen::VectorXd& ub = control_box_constraint->getUpperBound() - u; + const Eigen::VectorXd& x0 = k_[t]; // Initial guess + + cddp::BoxQPResult qp_result = boxqp_solver_.solve(Q_uu_reg, Q_u, lb, ub, x0); + + // TODO: Better status check + if (qp_result.status == BoxQPStatus::HESSIAN_NOT_PD || + qp_result.status == BoxQPStatus::NO_DESCENT) { + if (options_.debug) { + std::cerr << "CDDP: BoxQP failed at time step " << t << std::endl; + } + return false; } - int active_count_new = active_indices.size(); - - // Create new constraint matrices - Eigen::MatrixXd C_new = Eigen::MatrixXd::Zero(active_count_new, control_dim); - Eigen::MatrixXd D_new = Eigen::MatrixXd::Zero(active_count_new, state_dim); - - if (active_count_new > 0) - { - // Fill new constraint matrices with active constraints - for (int i = 0; i < active_count_new; ++i) - { - C_new.row(i) = grad_u_g.row(active_indices[i]); - D_new.row(i) = grad_x_g.row(active_indices[i]); + + // Extract solution + k = qp_result.x; + + // Compute feedback gain matrix + K = Eigen::MatrixXd::Zero(control_dim, state_dim); + if (qp_result.free.sum() > 0) { + // Get indices of free variables + std::vector free_idx; + for (int i = 0; i < control_dim; i++) { + if (qp_result.free(i)) { + free_idx.push_back(i); + } } - // Calculate feedback gains - Eigen::MatrixXd W = -(C_new * Q_uu_inv * C_new.transpose()).inverse() * (C_new * Q_uu_inv); - Eigen::MatrixXd H = Q_uu_inv * (Eigen::MatrixXd::Identity(control_dim, control_dim) - C_new.transpose() * W); - k = -H * Q_u; - K = -H * Q_ux + W.transpose() * D_new; - } - else - { - // If no active constraints remain, revert to unconstrained solution - Eigen::MatrixXd H = Q_uu.inverse(); - K = -H * Q_ux; - k = -H * Q_u; + // Extract relevant parts of Q_ux for free variables + Eigen::MatrixXd Q_ux_free(free_idx.size(), state_dim); + for (size_t i = 0; i < free_idx.size(); i++) { + Q_ux_free.row(i) = Q_ux_reg.row(free_idx[i]); + } + + // Compute gains for free variables using the LDLT factorization + Eigen::MatrixXd K_free = -qp_result.Hfree.solve(Q_ux_free); + + // Put back into full K matrix + for (size_t i = 0; i < free_idx.size(); i++) { + K.row(free_idx[i]) = K_free.row(i); + } } } - // Store Q-function matrices - Q_UU_[t] = Q_uu; - Q_UX_[t] = Q_ux; - Q_U_[t] = Q_u; + // Store feedforward and feedback gain + k_[t] = k; + K_[t] = K; // Compute value function approximation Eigen::Vector2d dV_step; dV_step << Q_u.dot(k), 0.5 * k.dot(Q_uu * k); dV_ = dV_ + dV_step; - V_X_[t] = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; - V_XX_[t] = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; - V_XX_[t] = 0.5 * (V_XX_[t] + V_XX_[t].transpose()); // Symmetrize Hessian + V_x = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; + V_xx = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; + V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize Hessian // Compute optimality gap (Inf-norm) for convergence check Qu_error = std::max(Qu_error, Q_u.lpNorm()); @@ -643,132 +558,167 @@ bool CDDP::solveBackwardPass() optimality_gap_ = Qu_error; } + if (options_.debug) { + std::cout << "Qu_error: " << Qu_error << std::endl; + std::cout << "dV: " << dV_.transpose() << std::endl; + } + return true; } -// Forward pass -bool CDDP::solveForwardPass() -{ - bool is_feasible = false; +ForwardPassResult CDDP::solveForwardPass(double alpha) { + // Prepare result struct + ForwardPassResult result; + result.success = false; + result.cost = std::numeric_limits::infinity(); + result.lagrangian = std::numeric_limits::infinity(); + result.alpha = alpha; + const int state_dim = system_->getStateDim(); const int control_dim = system_->getControlDim(); - // Extract control box constraint - auto control_box_constraint = getConstraint("ControlBoxConstraint"); - - int iter = 0; - double alpha = options_.backtracking_coeff; - - // Line-search iteration - for (iter = 0; iter < options_.max_line_search_iterations; ++iter) - { - // Initialize cost and constraints - double J_new = 0.0, dJ = 0.0, expected_dV = 0.0, gradient_norm = 0.0; - double L_new = 0.0; - - std::vector X_new = X_; - std::vector U_new = U_; - X_new[0] = initial_state_; - - for (int t = 0; t < horizon_; ++t) - { - // Get state and control - const Eigen::VectorXd &x = X_new[t]; - const Eigen::VectorXd &u = U_new[t]; - - // Deviation from the nominal trajectory - const Eigen::VectorXd &delta_x = x - X_[t]; - - // Extract Q-function matrices - const Eigen::VectorXd &Q_u = Q_U_[t]; - const Eigen::MatrixXd &Q_uu = Q_UU_[t]; - const Eigen::MatrixXd &Q_ux = Q_UX_[t]; - - const Eigen::VectorXd &q = alpha * Q_u + Q_ux * delta_x; // Gradient of QP objective + // Initialize trajectories + std::vector X_new = X_; + std::vector U_new = U_; + X_new[0] = initial_state_; + double J_new = 0.0; - // Lower and upper bounds - const Eigen::VectorXd& lb = control_box_constraint->getLowerBound() - u; - const Eigen::VectorXd& ub = control_box_constraint->getUpperBound() - u; - - // Create b vector - Eigen::VectorXd b(2 * control_dim); - b << ub, -lb; - - // Create A matrix - Eigen::MatrixXd A(2 * control_dim, control_dim); // Upper is identity, lower is -identity - A.setIdentity(); - A.block(control_dim, 0, control_dim, control_dim) = -Eigen::MatrixXd::Identity(control_dim, control_dim); + auto control_box_constraint = getConstraint("ControlBoxConstraint"); - QPSolverOptions options; - options.verbose = false; - QPSolver solver(options); + for (int t = 0; t < horizon_; ++t) { + const Eigen::VectorXd& x = X_new[t]; + const Eigen::VectorXd& u = U_new[t]; + const Eigen::VectorXd& delta_x = x - X_[t]; - solver.setDimensions(state_dim, 2*control_dim); - solver.setHessian(Q_uu); - solver.setGradient(q); - solver.setConstraints(A, b); + U_new[t] = u + alpha * k_[t] + K_[t] * delta_x; - QPResult result = solver.solve(); + if (control_box_constraint != nullptr) { + U_new[t] = control_box_constraint->clamp(U_new[t]); + } - // Extract solution - const Eigen::VectorXd &delta_u = result.x; + J_new += objective_->running_cost(x, U_new[t], t); + X_new[t + 1] = system_->getDiscreteDynamics(x, U_new[t]); + } + J_new += objective_->terminal_cost(X_new.back()); + double dJ = J_ - J_new; + double expected = -alpha * (dV_(0) + 0.5 * alpha * dV_(1)); + double reduction_ratio = expected > 0.0 ? dJ / expected : std::copysign(1.0, dJ); + + // Check if cost reduction is sufficient + result.success = reduction_ratio > options_.minimum_reduction_ratio; + result.state_sequence = X_new; + result.control_sequence = U_new; + result.cost = J_new; + result.lagrangian = J_new; + + return result; +} - // Extract solution - U_new[t] += delta_u; +void CDDP::printSolverInfo() +{ + std::cout << "\n"; + std::cout << "\033[34m"; // Set text color to blue + std::cout << "+---------------------------------------------------------+" << std::endl; + std::cout << "| ____ ____ ____ ____ _ ____ |" << std::endl; + std::cout << "| / ___| _ \\| _ \\| _ \\ (_)_ __ / ___| _ _ |" << std::endl; + std::cout << "| | | | | | | | | | |_) | | | '_ \\ | | _| |_ _| |_ |" << std::endl; + std::cout << "| | |___| |_| | |_| | __/ | | | | | | |__|_ _|_ _| |" << std::endl; + std::cout << "| \\____|____/|____/|_| |_|_| |_| \\____||_| |_| |" << std::endl; + std::cout << "+---------------------------------------------------------+" << std::endl; + std::cout << "\n"; + std::cout << "Constrained Differential Dynamic Programming\n"; + std::cout << "Author: Tomo Sasaki (@astomodynamics)\n"; + std::cout << "----------------------------------------------------------\n"; + std::cout << "\033[0m"; // Reset text color + std::cout << "\n"; +} - // Compute cost - J_new += objective_->running_cost(x, U_new[t], t); +void CDDP::printOptions(const CDDPOptions &options) +{ + std::cout << "\n========================================\n"; + std::cout << " CDDP Options\n"; + std::cout << "========================================\n"; + + std::cout << "Cost Tolerance: " << std::setw(10) << options.cost_tolerance << "\n"; + std::cout << "Grad Tolerance: " << std::setw(10) << options.grad_tolerance << "\n"; + std::cout << "Max Iterations: " << std::setw(10) << options.max_iterations << "\n"; + std::cout << "Max CPU Time: " << std::setw(10) << options.max_cpu_time << "\n"; + + std::cout << "\nLine Search:\n"; + std::cout << " Max Iterations: " << std::setw(5) << options.max_line_search_iterations << "\n"; + std::cout << " Backtracking Coeff: " << std::setw(5) << options.backtracking_coeff << "\n"; + std::cout << " Backtracking Min: " << std::setw(5) << options.backtracking_min << "\n"; + std::cout << " Backtracking Factor: " << std::setw(5) << options.backtracking_factor << "\n"; + + std::cout << "\nLog-Barrier:\n"; + std::cout << " Barrier Coeff: " << std::setw(5) << options.barrier_coeff << "\n"; + std::cout << " Barrier Factor: " << std::setw(5) << options.barrier_factor << "\n"; + std::cout << " Barrier Tolerance: " << std::setw(5) << options.barrier_tolerance << "\n"; + std::cout << " Relaxation Coeff: " << std::setw(5) << options.relaxation_coeff << "\n"; + + std::cout << "\nRegularization:\n"; + std::cout << " Regularization Type: " << options.regularization_type << "\n"; + std::cout << " Regularization State: " << std::setw(5) << options.regularization_state << "\n"; + std::cout << " Regularization State Step: " << std::setw(5) << options.regularization_state_step << "\n"; + std::cout << " Regularization State Max: " << std::setw(5) << options.regularization_state_max << "\n"; + std::cout << " Regularization State Min: " << std::setw(5) << options.regularization_state_min << "\n"; + std::cout << " Regularization State Factor: " << std::setw(5) << options.regularization_state_factor << "\n"; + + std::cout << " Regularization Control: " << std::setw(5) << options.regularization_control << "\n"; + std::cout << " Regularization Control Step: " << std::setw(5) << options.regularization_control_step << "\n"; + std::cout << " Regularization Control Max: " << std::setw(5) << options.regularization_control_max << "\n"; + std::cout << " Regularization Control Min: " << std::setw(5) << options.regularization_control_min << "\n"; + std::cout << " Regularization Control Factor: " << std::setw(5) << options.regularization_control_factor << "\n"; + + std::cout << "\nOther:\n"; + std::cout << " Print Iterations: " << (options.verbose ? "Yes" : "No") << "\n"; + std::cout << " iLQR: " << (options.is_ilqr ? "Yes" : "No") << "\n"; + std::cout << " Use Parallel: " << (options.use_parallel ? "Yes" : "No") << "\n"; + std::cout << " Num Threads: " << options.num_threads << "\n"; + + std::cout << "========================================\n\n"; +} - // Compute new state - X_new[t + 1] = system_->getDiscreteDynamics(x, U_new[t]); - } - J_new += objective_->terminal_cost(X_new.back()); +void CDDP::printIteration(int iter, double cost, double lagrangian, double grad_norm, + double lambda_state, double lambda_control, double step_size) +{ + // Print header for better readability every 10 iterations + if (iter % 10 == 0) + { + std::cout << std::setw(10) << "Iteration" + << std::setw(15) << "Objective" + << std::setw(15) << "Lagrangian" + << std::setw(15) << "Grad Norm" + << std::setw(15) << "Step Size" + << std::setw(15) << "Reg (State)" + << std::setw(15) << "Reg (Control)" + << std::endl; + std::cout << std::string(95, '-') << std::endl; + } - // Calculate Cost Reduction - dJ = J_ - J_new; + // Print iteration details + std::cout << std::setw(10) << iter + << std::setw(15) << std::setprecision(6) << cost + << std::setw(15) << std::setprecision(6) << lagrangian + << std::setw(15) << std::setprecision(6) << grad_norm + << std::setw(15) << std::setprecision(6) << step_size + << std::setw(15) << std::setprecision(6) << lambda_state + << std::setw(15) << std::setprecision(6) << lambda_control + << std::endl; +} - double expected = -alpha * (dV_(0) + 0.5 * alpha * dV_(1)); - double reduction_ratio = 0.0; - if (expected > 0.0) - { - reduction_ratio = dJ / expected; - } - else - { - reduction_ratio = std::copysign(1.0, dJ); - std::cout << "Expected improvement is not positive" << std::endl; - } - if (reduction_ratio > options_.minimum_reduction_ratio) - { - // Update state and control - X_ = X_new; - U_ = U_new; - J_ = J_new; - dJ_ = dJ; - alpha_ = alpha; - // barrier_coeff_ = options_.barrier_coeff / 10.0; - return true; - } - else - { - alpha *= options_.backtracking_factor; - } +void CDDP::printSolution(const CDDPSolution &solution) +{ + std::cout << "\n========================================\n"; + std::cout << " CDDP Solution\n"; + std::cout << "========================================\n"; - // // Check constraints - // is_feasible = true; - // for (int t = 0; t < horizon_; ++t) { - // const Eigen::VectorXd& u = U_new[t]; - // // Check control box constraint - // if (control_box_constraint != constraint_set_.end()) { - // if (!control_box_constraint->second->isFeasible(u)) { - // is_feasible = false; - // break; - // } - // } - // } - } + std::cout << "Converged: " << (solution.converged ? "Yes" : "No") << "\n"; + std::cout << "Iterations: " << solution.iterations << "\n"; + std::cout << "Solve Time: " << std::setprecision(4) << solution.solve_time << " micro sec\n"; + std::cout << "Final Cost: " << std::setprecision(6) << solution.cost_sequence.back() << "\n"; // Assuming cost_sequence is not empty - return false; // Or false if forward pass fails + std::cout << "========================================\n\n"; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/cddp_core/logddp_core.cpp b/src/cddp_core/logddp_core.cpp index 562ba84..eee48b9 100644 --- a/src/cddp_core/logddp_core.cpp +++ b/src/cddp_core/logddp_core.cpp @@ -34,27 +34,30 @@ namespace cddp CDDPSolution CDDP::solveLogCDDP() { - // Initialize CDDP solver - initializeCDDP(); + // Initialize if not done + if (!initialized_) { + initializeCDDP(); + } - if (options_.verbose) - { - printOptions(options_); + if (!initialized_) { + std::cerr << "CDDP: Initialization failed" << std::endl; + throw std::runtime_error("CDDP: Initialization failed"); } - // Initialize solution + // Prepare solution struct CDDPSolution solution; solution.converged = false; + solution.alpha = alpha_; + solution.iterations = 0; + solution.solve_time = 0.0; solution.time_sequence.reserve(horizon_ + 1); - for (int t = 0; t <= horizon_; ++t) - { + for (int t = 0; t <= horizon_; ++t) { solution.time_sequence.push_back(timestep_ * t); } - solution.cost_sequence.reserve(options_.max_iterations); - solution.lagrangian_sequence.reserve(options_.max_iterations); - - // Evaluate initial cost - J_ = objective_->evaluate(X_, U_); + solution.control_sequence.reserve(horizon_); + solution.state_sequence.reserve(horizon_ + 1); + solution.cost_sequence.reserve(options_.max_iterations); // Reserve space for efficiency + solution.lagrangian_sequence.reserve(options_.max_iterations); // Reserve space for efficiency solution.cost_sequence.push_back(J_); // Evaluate Lagrangian @@ -70,6 +73,7 @@ CDDPSolution CDDP::solveLogCDDP() L_ += log_barrier_->evaluate(*constraint.second, X_[t], U_[t]); } } + solution.lagrangian_sequence.push_back(L_); if (options_.verbose) { @@ -83,6 +87,7 @@ CDDPSolution CDDP::solveLogCDDP() // Main loop of CDDP while (iter < options_.max_iterations) { ++iter; + solution.iterations = iter; // Check maximum CPU time if (options_.max_cpu_time > 0) { @@ -136,28 +141,24 @@ CDDPSolution CDDP::solveLogCDDP() // 2. Forward pass (either single-threaded or multi-threaded) ForwardPassResult best_result; best_result.cost = std::numeric_limits::infinity(); - bool forward_pass_feasible = false; + best_result.lagrangian = std::numeric_limits::infinity(); bool forward_pass_success = false; if (!options_.use_parallel) { // Single-threaded execution with early termination for (double alpha : alphas_) { - ForwardPassResult result = solveCLDDPForwardPass(alpha); + ForwardPassResult result = solveLogCDDPForwardPass(alpha); if (result.success && result.cost < best_result.cost) { best_result = result; - forward_pass_feasible = true; + forward_pass_success = true; // Check for early termination - double expected_cost_reduction = -alpha * (dV_(0) + 0.5 * alpha * dV_(1)); - if (expected_cost_reduction > 0.0) { - double cost_reduction_ratio = (J_ - result.cost) / expected_cost_reduction; - if (cost_reduction_ratio > options_.minimum_reduction_ratio) { - if (options_.debug) { - std::cout << "CDDP: Early termination due to successful forward pass" << std::endl; - } - break; + if (result.success) { + if (options_.debug) { + std::cout << "CDDP: Early termination due to successful forward pass" << std::endl; } + break; } } } @@ -180,7 +181,7 @@ CDDPSolution CDDP::solveLogCDDP() ForwardPassResult result = future.get(); if (result.success && result.cost < best_result.cost) { best_result = result; - forward_pass_feasible = true; + forward_pass_success = true; } } } catch (const std::exception& e) { @@ -193,41 +194,16 @@ CDDPSolution CDDP::solveLogCDDP() } // Update solution if a feasible forward pass was found - if (forward_pass_feasible) { + if (forward_pass_success) { if (options_.debug) { std::cout << "Best cost: " << best_result.cost << std::endl; std::cout << "Best alpha: " << best_result.alpha << std::endl; } - - double expected_cost_reduction = -best_result.alpha * (dV_(0) + 0.5 * best_result.alpha * dV_(1)); - double cost_reduction_ratio; - - // Check if cost reduction is positive - if (expected_cost_reduction > 0.0) { - cost_reduction_ratio = (J_ - best_result.cost) / expected_cost_reduction; - } else { - cost_reduction_ratio = std::copysign(1.0, J_ - best_result.cost); - if (options_.debug) { - std::cerr << "CDDP: Expected cost reduction is non-positive" << std::endl; - } - } - - // Check if cost reduction is sufficient - if (cost_reduction_ratio > options_.minimum_reduction_ratio) { - forward_pass_success = true; - } else { - alpha_ = std::numeric_limits::infinity(); - if (options_.debug) { - std::cerr << "CDDP: Cost reduction ratio is too small" << std::endl; - } - } - } - - if (forward_pass_success) { X_ = best_result.state_sequence; U_ = best_result.control_sequence; dJ_ = J_ - best_result.cost; J_ = best_result.cost; + dL_ = L_ - best_result.lagrangian; L_ = best_result.lagrangian; alpha_ = best_result.alpha; solution.cost_sequence.push_back(J_); @@ -261,7 +237,7 @@ CDDPSolution CDDP::solveLogCDDP() // Print iteration information if (options_.verbose) { - printIteration(iter, J_, L_, optimality_gap_, expected_, regularization_control_, alpha_); + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); } } auto end_time = std::chrono::high_resolution_clock::now(); @@ -271,10 +247,11 @@ CDDPSolution CDDP::solveLogCDDP() solution.state_sequence = X_; solution.control_sequence = U_; solution.alpha = alpha_; - solution.iterations = iter; solution.solve_time = duration.count(); // Time in microseconds - printSolution(solution); + if (options_.verbose) { + printSolution(solution); + } return solution; } @@ -287,9 +264,9 @@ bool CDDP::solveLogCDDPBackwardPass() { // Get control box constraint auto control_box_constraint = getConstraint("ControlBoxConstraint"); - // Terminal cost and derivatives - V_X_.back() = objective_->getFinalCostGradient(X_.back()); - V_XX_.back() = objective_->getFinalCostHessian(X_.back()); + // Terminal cost and its derivatives] + Eigen::VectorXd V_x = objective_->getFinalCostGradient(X_.back()); + Eigen::MatrixXd V_xx = objective_->getFinalCostHessian(X_.back()); // Pre-allocate matrices Eigen::MatrixXd A(state_dim, state_dim); @@ -306,61 +283,38 @@ bool CDDP::solveLogCDDPBackwardPass() { Eigen::MatrixXd K(control_dim, state_dim); dV_ = Eigen::Vector2d::Zero(); - // Create BoxQP solver - cddp::BoxQPOptions qp_options; - qp_options.verbose = false; - qp_options.maxIter = 1000; - // qp_options.eps_abs = 1e-3; - // qp_options.eps_rel = 1e-2; - cddp::BoxQPSolver qp_solver(qp_options); - double Qu_error = 0.0; - // Backward pass - for (int t = horizon_ - 1; t >= 0; --t) - { - const Eigen::VectorXd &x = X_[t]; - const Eigen::VectorXd &u = U_[t]; - - // Get value function - const Eigen::VectorXd &V_x = V_X_[t + 1]; - const Eigen::MatrixXd &V_xx = V_XX_[t + 1]; + // Backward Riccati recursion + for (int t = horizon_ - 1; t >= 0; --t) { + // Get state and control + const Eigen::VectorXd& x = X_[t]; + const Eigen::VectorXd& u = U_[t]; - // Get dynamics Jacobians + // Get continuous dynamics Jacobians const auto [Fx, Fu] = system_->getJacobians(x, u); - // Discrete dynamics - A = timestep_ * Fx; + // Convert continuous dynamics to discrete time + A = timestep_ * Fx; A.diagonal().array() += 1.0; B = timestep_ * Fu; - // Get cost derivatives + // Get cost and its derivatives double l = objective_->running_cost(x, u, t); auto [l_x, l_u] = objective_->getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = objective_->getRunningCostHessians(x, u, t); - // // Add log barrier terms - // const double barrier_cost = log_barrier_->evaluate(*control_box_constraint, x, u); - // l += barrier_cost; - - // // Get barrier gradients - // auto [barrier_x, barrier_u] = log_barrier_->getGradients(*control_box_constraint, x, u, barrier_cost); - // l_x += barrier_x; - // l_u += barrier_u; - - // // Get barrier Hessians - // auto [barrier_xx, barrier_uu, barrier_ux] = log_barrier_->getHessians(*control_box_constraint, x, u, barrier_cost); - // l_xx += barrier_xx; - // l_uu += barrier_uu; - // l_ux += barrier_ux; - - // Q-function matrices + // Compute Q-function matrices Q_x = l_x + A.transpose() * V_x; Q_u = l_u + B.transpose() * V_x; Q_xx = l_xx + A.transpose() * V_xx * A; Q_ux = l_ux + B.transpose() * V_xx * A; Q_uu = l_uu + B.transpose() * V_xx * B; + // TODO: Apply Cholesky decomposition to Q_uu later? + // // Symmetrize Q_uu for Cholesky decomposition + // Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); + if (options_.regularization_type == "state" || options_.regularization_type == "both") { Q_ux_reg = l_ux + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * A; Q_uu_reg = l_uu + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * B; @@ -375,10 +329,8 @@ bool CDDP::solveLogCDDPBackwardPass() { // Check eigenvalues of Q_uu Eigen::EigenSolver es(Q_uu_reg); - Eigen::VectorXd eigenvalues = es.eigenvalues().real(); + const Eigen::VectorXd& eigenvalues = es.eigenvalues().real(); if (eigenvalues.minCoeff() <= 0) { - eigenvalues = es.eigenvalues().real(); - if (options_.debug) { std::cerr << "CDDP: Q_uu is still not positive definite" << std::endl; } @@ -389,17 +341,15 @@ bool CDDP::solveLogCDDPBackwardPass() { const Eigen::MatrixXd &H = Q_uu_reg.inverse(); k = -H * Q_u; K = -H * Q_ux_reg; - if (options_.debug) { - std::cout << "No control box constraint" << std::endl; - } } else { // Solve QP by boxQP - Eigen::VectorXd lb = control_box_constraint->getLowerBound() - u; - Eigen::VectorXd ub = control_box_constraint->getUpperBound() - u; - Eigen::VectorXd x0 = Eigen::VectorXd::Zero(control_dim); // Initial guess + const Eigen::VectorXd& lb = control_box_constraint->getLowerBound() - u; + const Eigen::VectorXd& ub = control_box_constraint->getUpperBound() - u; + const Eigen::VectorXd& x0 = k_[t]; // Initial guess - cddp::BoxQPResult qp_result = qp_solver.solve(Q_uu_reg, Q_u, lb, ub, x0); + cddp::BoxQPResult qp_result = boxqp_solver_.solve(Q_uu_reg, Q_u, lb, ub, x0); + // TODO: Better status check if (qp_result.status == BoxQPStatus::HESSIAN_NOT_PD || qp_result.status == BoxQPStatus::NO_DESCENT) { if (options_.debug) { @@ -446,9 +396,9 @@ bool CDDP::solveLogCDDPBackwardPass() { Eigen::Vector2d dV_step; dV_step << Q_u.dot(k), 0.5 * k.dot(Q_uu * k); dV_ = dV_ + dV_step; - V_X_[t] = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; - V_XX_[t] = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; - V_XX_[t] = 0.5 * (V_XX_[t] + V_XX_[t].transpose()); // Symmetrize Hessian + V_x = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u; + V_xx = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux; + V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize Hessian // Compute optimality gap (Inf-norm) for convergence check Qu_error = std::max(Qu_error, Q_u.lpNorm()); @@ -461,11 +411,18 @@ bool CDDP::solveLogCDDPBackwardPass() { std::cout << "Qu_error: " << Qu_error << std::endl; std::cout << "dV: " << dV_.transpose() << std::endl; } + return true; } ForwardPassResult CDDP::solveLogCDDPForwardPass(double alpha) { + // Prepare result struct ForwardPassResult result; + result.success = false; + result.cost = std::numeric_limits::infinity(); + result.lagrangian = std::numeric_limits::infinity(); + result.alpha = alpha; + const int state_dim = system_->getStateDim(); const int control_dim = system_->getControlDim(); @@ -508,7 +465,6 @@ ForwardPassResult CDDP::solveLogCDDPForwardPass(double alpha) { result.control_sequence = U_new; result.cost = J_new; result.lagrangian = L_new; - result.alpha = alpha; return result; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5f0b65d..cb85ec0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -83,9 +83,9 @@ add_executable(test_cddp_core cddp_core/test_cddp_core.cpp) target_link_libraries(test_cddp_core gtest gmock gtest_main cddp) gtest_discover_tests(test_cddp_core) -add_executable(test_clddp_core cddp_core/test_clddp_core.cpp) -target_link_libraries(test_clddp_core gtest gmock gtest_main cddp) -gtest_discover_tests(test_clddp_core) +add_executable(test_asddp_core cddp_core/test_asddp_core.cpp) +target_link_libraries(test_asddp_core gtest gmock gtest_main cddp) +gtest_discover_tests(test_asddp_core) add_executable(test_logcddp_core cddp_core/test_logcddp_core.cpp) target_link_libraries(test_logcddp_core gtest gmock gtest_main cddp) diff --git a/tests/cddp_core/test_clddp_core.cpp b/tests/cddp_core/test_asddp_core.cpp similarity index 94% rename from tests/cddp_core/test_clddp_core.cpp rename to tests/cddp_core/test_asddp_core.cpp index af09a34..109d97e 100644 --- a/tests/cddp_core/test_clddp_core.cpp +++ b/tests/cddp_core/test_asddp_core.cpp @@ -25,7 +25,7 @@ namespace plt = matplotlibcpp; namespace fs = std::filesystem; -TEST(CDDPTest, SolveCLDDP) { +TEST(CDDPTest, SolveASCDDP) { // Problem parameters int state_dim = 3; int control_dim = 2; @@ -55,8 +55,20 @@ TEST(CDDPTest, SolveCLDDP) { Eigen::VectorXd initial_state(state_dim); initial_state << 0.0, 0.0, M_PI/4.0; + // Create CDDP Options + cddp::CDDPOptions options; + options.max_iterations = 20; + options.max_cpu_time = 1e-1; + // // Create CDDP solver - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep); + cddp::CDDP cddp_solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique(timestep, integration_type), + std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), + options); cddp_solver.setDynamicalSystem(std::move(system)); cddp_solver.setObjective(std::move(objective)); @@ -70,9 +82,6 @@ TEST(CDDPTest, SolveCLDDP) { auto constraint = cddp_solver.getConstraint("ControlBoxConstraint"); // Set options - cddp::CDDPOptions options; - options.max_iterations = 20; - options.max_cpu_time = 1e-1; cddp_solver.setOptions(options); // Set initial trajectory @@ -81,7 +90,7 @@ TEST(CDDPTest, SolveCLDDP) { cddp_solver.setInitialTrajectory(X, U); // // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); + cddp::CDDPSolution solution = cddp_solver.solveASCDDP(); ASSERT_TRUE(solution.converged); diff --git a/tests/cddp_core/test_boxqp.cpp b/tests/cddp_core/test_boxqp.cpp index 49e7fe2..6434344 100644 --- a/tests/cddp_core/test_boxqp.cpp +++ b/tests/cddp_core/test_boxqp.cpp @@ -120,6 +120,123 @@ TEST(QPSolver, ComparisonTest) { } } +TEST(BoxQPSolver, LargeDimensionTest) { + // Problem setup + const int n = 15; // variables + VectorXd box_lb = VectorXd::Constant(n, -2.0); + VectorXd box_ub = VectorXd::Constant(n, 2.0); + Eigen::Matrix Q; + Q << + // Row 0 + 4.12533748e+00, 1.97764831e-01, -3.61210691e-01, 8.00623610e-01, 7.62345888e-01, -1.06898343e-01, + 1.30339351e-01, -1.13259907e-01, -6.96950858e-01, 5.34670445e-01, 5.09630616e-01, 1.60610297e+00, + 4.62892147e-01, -4.16042523e-02, 1.15372788e+00, + + // Row 1 + 1.97764831e-01, 6.09416699e+00, 1.24916711e-01, 3.03114135e-01, 8.33526744e-02, -7.50650156e-01, + 3.32367503e-01, -8.07693384e-01, -3.63386713e-01, 5.97739905e-02, 1.12281290e-01, 7.82962364e-01, + -4.31227827e-01, -7.78651128e-01, 3.53467184e-01, + + // Row 2 + -3.61210691e-01, 1.24916711e-01, 5.50001670e+00, -1.15999299e+00, 2.52935791e-02, 6.78993972e-01, + -4.26688352e-01, 2.79368593e-01, -1.20240548e+00, 5.81617495e-03, -6.60420998e-01, -8.52349453e-01, + -2.21853251e-01, 3.25270059e-01, -4.93159468e-01, + + // Row 3 + 8.00623610e-01, 3.03114135e-01, -1.15999299e+00, 4.77842560e+00, 2.32184776e-04, -7.16724074e-01, + 1.17921496e+00, -2.44985365e-01, -6.36689772e-01, -1.18475001e+00, -1.06986243e+00, 1.15671298e+00, + 3.28904560e-01, -1.99200945e-02, -6.51702604e-01, + + // Row 4 + 7.62345888e-01, 8.33526744e-02, 2.52935791e-02, 2.32184776e-04, 4.40332914e+00, 4.66324836e-01, + -7.95405086e-01, -2.58389526e-01, -2.64881040e-01, -9.80426131e-01, 7.14939428e-01, -8.29804991e-02, + -9.83858675e-01, -1.10025256e-01, -4.81364585e-01, + + // Row 5 + -1.06898343e-01, -7.50650156e-01, 6.78993972e-01, -7.16724074e-01, 4.66324836e-01, 3.33222011e+00, + -1.74735819e-01, 7.60206374e-01, 1.38069876e-01, -1.14377493e+00, 2.49459813e-01, -3.03438457e-01, + 4.87653948e-01, -1.41316602e+00, -5.60885984e-01, + + // Row 6 + 1.30339351e-01, 3.32367503e-01, -4.26688352e-01, 1.17921496e+00, -7.95405086e-01, -1.74735819e-01, + 4.67874106e+00, -2.86394759e-01, -3.00259011e-01, 2.58571169e-01, 2.28293192e-01, -1.12559618e-01, + -2.78415986e-01, -2.69818813e-01, -1.16263409e+00, + + // Row 7 + -1.13259907e-01, -8.07693384e-01, 2.79368593e-01, -2.44985365e-01, -2.58389526e-01, 7.60206374e-01, + -2.86394759e-01, 5.41952411e+00, 4.44979488e-01, 9.16500690e-01, 8.42587916e-02, 2.27753618e-01, + 1.52053204e+00, 2.93800655e-01, 1.77636682e-01, + + // Row 8 + -6.96950858e-01, -3.63386713e-01, -1.20240548e+00, -6.36689772e-01, -2.64881040e-01, 1.38069876e-01, + -3.00259011e-01, 4.44979488e-01, 5.07267790e+00, -1.62937574e-01, 1.36247527e-01, -1.82020551e-01, + 1.15182717e+00, -7.22601223e-01, 1.48759838e+00, + + // Row 9 + 5.34670445e-01, 5.97739905e-02, 5.81617495e-03, -1.18475001e+00, -9.80426131e-01, -1.14377493e+00, + 2.58571169e-01, 9.16500690e-01, -1.62937574e-01, 5.46146888e+00, -2.05325482e-01, -9.36657857e-02, + -1.71213726e-02, 5.30645010e-01, 2.73102921e-01, + + // Row 10 + 5.09630616e-01, 1.12281290e-01, -6.60420998e-01, -1.06986243e+00, 7.14939428e-01, 2.49459813e-01, + 2.28293192e-01, 8.42587916e-02, 1.36247527e-01, -2.05325482e-01, 5.23868157e+00, -5.17103373e-01, + -8.16166203e-01, -4.75320773e-01, 7.70793551e-02, + + // Row 11 + 1.60610297e+00, 7.82962364e-01, -8.52349453e-01, 1.15671298e+00, -8.29804991e-02, -3.03438457e-01, + -1.12559618e-01, 2.27753618e-01, -1.82020551e-01, -9.36657857e-02, -5.17103373e-01, 4.34814350e+00, + -1.42655295e+00, -1.51741483e-01, 5.85827374e-01, + + // Row 12 + 4.62892147e-01, -4.31227827e-01, -2.21853251e-01, 3.28904560e-01, -9.83858675e-01, 4.87653948e-01, + -2.78415986e-01, 1.52053204e+00, 1.15182717e+00, -1.71213726e-02, -8.16166203e-01, -1.42655295e+00, + 6.06160017e+00, -5.47030759e-01, -1.17969147e+00, + + // Row 13 + -4.16042523e-02, -7.78651128e-01, 3.25270059e-01, -1.99200945e-02, -1.10025256e-01, -1.41316602e+00, + -2.69818813e-01, 2.93800655e-01, -7.22601223e-01, 5.30645010e-01, -4.75320773e-01, -1.51741483e-01, + -5.47030759e-01, 4.51860738e+00, 1.44827840e+00, + + // Row 14 + 1.15372788e+00, 3.53467184e-01, -4.93159468e-01, -6.51702604e-01, -4.81364585e-01, -5.60885984e-01, + -1.16263409e+00, 1.77636682e-01, 1.48759838e+00, 2.73102921e-01, 7.70793551e-02, 5.85827374e-01, + -1.17969147e+00, 1.44827840e+00, 5.10583464e+00; + + Eigen::Matrix q; + q << -0.86050975, + -1.63121951, + -0.30147231, + -0.25623270, + 0.85766191, + -0.11059050, + -0.43243198, + 1.07703747, + -0.22482656, + -0.57624182, + 0.57460892, + -0.48982822, + 0.65880214, + -0.59691711, + -0.22295918; + + BoxQPOptions options; + options.verbose = false; + BoxQPSolver solver(options); + + auto start_time = std::chrono::high_resolution_clock::now(); + BoxQPResult result = solver.solve(Q, q, box_lb, box_ub); + auto end_time = std::chrono::high_resolution_clock::now(); + std::chrono::duration elapsed = end_time - start_time; + + printResults("BoxQP", result.x, result.final_value, + elapsed.count(), static_cast(result.status)); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + /* $ ./tests/test_qp_solvers [==========] Running 1 test from 1 test suite. [----------] Global test environment set-up. diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index 179f7d6..4e4bc01 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -55,27 +55,38 @@ TEST(CDDPTest, Solve) { Eigen::VectorXd initial_state(state_dim); initial_state << 0.0, 0.0, M_PI/4.0; + // Create CDDP Options + cddp::CDDPOptions options; + options.max_iterations = 20; + options.cost_tolerance = 1e-2; + options.use_parallel = true; + options.num_threads = 10; + options.verbose = true; + options.debug = false; + // Create CDDP solver - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep); + cddp::CDDP cddp_solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique(timestep, integration_type), + std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), + options); cddp_solver.setDynamicalSystem(std::move(system)); cddp_solver.setObjective(std::move(objective)); // Define constraints Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -1.0, -M_PI; + control_lower_bound << -1.0, -3.1415; Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.0, M_PI; + control_upper_bound << 1.0, 3.1415; // Add the constraint to the solver cddp_solver.addConstraint(std::string("ControlBoxConstraint"), std::make_unique(control_lower_bound, control_upper_bound)); auto constraint = cddp_solver.getConstraint("ControlBoxConstraint"); // Set options - cddp::CDDPOptions options; - options.max_iterations = 30; - options.cost_tolerance = 1e-2; - options.use_parallel = false; - options.num_threads = 1; cddp_solver.setOptions(options); // Set initial trajectory diff --git a/tests/cddp_core/test_logcddp_core.cpp b/tests/cddp_core/test_logcddp_core.cpp index f74edb2..131337c 100644 --- a/tests/cddp_core/test_logcddp_core.cpp +++ b/tests/cddp_core/test_logcddp_core.cpp @@ -55,8 +55,20 @@ TEST(CDDPTest, SolveLogCDDP) { Eigen::VectorXd initial_state(state_dim); initial_state << 0.0, 0.0, M_PI/4.0; + // Create CDDP options + cddp::CDDPOptions options; + options.max_iterations = 10; + options.cost_tolerance = 1e-1; + // Create CDDP solver - cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep); + cddp::CDDP cddp_solver( + initial_state, + goal_state, + horizon, + timestep, + std::make_unique(timestep, integration_type), + std::make_unique(Q, R, Qf, goal_state, empty_reference_states, timestep), + options); cddp_solver.setDynamicalSystem(std::move(system)); cddp_solver.setObjective(std::move(objective)); @@ -71,9 +83,6 @@ TEST(CDDPTest, SolveLogCDDP) { auto constraint = cddp_solver.getConstraint("ControlBoxConstraint"); // Set options - cddp::CDDPOptions options; - options.max_iterations = 10; - options.cost_tolerance = 1e-1; cddp_solver.setOptions(options); // Set initial trajectory