diff --git a/CMakeLists.txt b/CMakeLists.txt index ba18d38..60c40d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -192,10 +192,12 @@ set(cddp_core_srcs src/cddp_core/dynamical_system.cpp src/cddp_core/objective.cpp src/cddp_core/constraint.cpp + src/cddp_core/barrier.cpp src/cddp_core/helper.cpp 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/neural_dynamical_system.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f6439d3..082328b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -13,7 +13,6 @@ # limitations under the License. # CMakeLists.txt for the CDDP examples - add_executable(cddp_bicycle cddp_bicycle.cpp) target_link_libraries(cddp_bicycle cddp) @@ -38,7 +37,13 @@ target_link_libraries(cddp_pendulum cddp) add_executable(cddp_quadrotor cddp_quadrotor.cpp) target_link_libraries(cddp_quadrotor cddp) -# Neural dynamics example +# Ipopt examples +if (CDDP_CPP_CASADI) + add_executable(ipopt_car ipopt_car.cpp) + target_link_libraries(ipopt_car cddp) +endif() + +# Neural dynamics examples add_executable(prepare_pendulum neural_dynamics/prepare_pendulum.cpp) target_link_libraries(prepare_pendulum cddp) diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp index 84e8d98..8fa7548 100644 --- a/examples/cddp_car.cpp +++ b/examples/cddp_car.cpp @@ -135,15 +135,15 @@ int main() { Animation::AnimationConfig config; config.width = 800; config.height = 800; - config.frame_skip = 5; // Save every 5th frame - config.frame_delay = 10; // 10/100 = 0.1 seconds between frames + config.frame_skip = 5; + config.frame_delay = 10; Animation animation(config); // Problem parameters int state_dim = 4; // [x y theta v] int control_dim = 2; // [wheel_angle acceleration] - int horizon = 500; // Same as MATLAB example - double timestep = 0.03; // h = 0.03 from MATLAB + int horizon = 500; + double timestep = 0.03; std::string integration_type = "euler"; // Random number generator setup @@ -152,13 +152,13 @@ int main() { std::normal_distribution d(0.0, 0.1); // Create car instance - double wheelbase = 2.0; // d = 2.0 from MATLAB example + double wheelbase = 2.0; std::unique_ptr system = std::make_unique(timestep, wheelbase, integration_type); // Initial and goal states Eigen::VectorXd initial_state(state_dim); - initial_state << 1.0, 1.0, 1.5*M_PI, 0.0; // [1; 1; pi*3/2; 0] from MATLAB + initial_state << 1.0, 1.0, 1.5*M_PI, 0.0; Eigen::VectorXd goal_state(state_dim); goal_state << 0.0, 0.0, 0.0, 0.0; // NOT USED IN THIS EXAMPLE @@ -189,7 +189,7 @@ int main() { options.grad_tolerance = 1e-4; options.regularization_type = "control"; // options.regularization_control = 1.0; - options.debug = true; + options.debug = false; options.use_parallel = true; options.num_threads = 10; cddp_solver.setOptions(options); @@ -225,8 +225,23 @@ int main() { // Solve the problem cddp::CDDPSolution solution = cddp_solver.solve(); - // cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); - // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP(); + // ======================================== + // CDDP Solution + // ======================================== + // Converged: Yes + // Iterations: 157 + // Solve Time: 5.507e+05 micro sec + // Final Cost: 1.90517 + // ======================================== + // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP(); + // ======================================== + // CDDP Solution + // ======================================== + // Converged: Yes + // Iterations: 153 + // Solve Time: 5.441e+05 micro sec + // Final Cost: 1.90517 + // ======================================== // Extract solution trajectories auto X_sol = solution.state_sequence; diff --git a/examples/cddp_dubins_car.cpp b/examples/cddp_dubins_car.cpp index 7d63598..b6062af 100644 --- a/examples/cddp_dubins_car.cpp +++ b/examples/cddp_dubins_car.cpp @@ -70,6 +70,8 @@ int main() { // Set options cddp::CDDPOptions options; options.max_iterations = 10; + options.barrier_coeff = 1e-2; + options.barrier_factor = 0.1; cddp_solver.setOptions(options); // Set initial trajectory @@ -78,7 +80,8 @@ int main() { cddp_solver.setInitialTrajectory(X, U); // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solve(); + // cddp::CDDPSolution solution = cddp_solver.solve("CLCDDP"); + cddp::CDDPSolution solution = cddp_solver.solve("LogCDDP"); // Extract solution auto X_sol = solution.state_sequence; // size: horizon + 1 diff --git a/examples/ipopt_car.cpp b/examples/ipopt_car.cpp new file mode 100644 index 0000000..0199123 --- /dev/null +++ b/examples/ipopt_car.cpp @@ -0,0 +1,286 @@ +#include +#include +#include +#include +#include +#include +#include "animation.hpp" + +namespace plt = matplotlibcpp; + +void plotCarBox(const Eigen::VectorXd& state, const Eigen::VectorXd& control, + double length, double width, const std::string& color) { + double x = state(0); + double y = state(1); + double theta = state(2); + + // Compute car corners + std::vector car_x(5), car_y(5); + + // Front right + car_x[0] = x + length/2 * cos(theta) - width/2 * sin(theta); + car_y[0] = y + length/2 * sin(theta) + width/2 * cos(theta); + + // Front left + car_x[1] = x + length/2 * cos(theta) + width/2 * sin(theta); + car_y[1] = y + length/2 * sin(theta) - width/2 * cos(theta); + + // Rear left + car_x[2] = x - length/2 * cos(theta) + width/2 * sin(theta); + car_y[2] = y - length/2 * sin(theta) - width/2 * cos(theta); + + // Rear right + car_x[3] = x - length/2 * cos(theta) - width/2 * sin(theta); + car_y[3] = y - length/2 * sin(theta) + width/2 * cos(theta); + + // Close the polygon + car_x[4] = car_x[0]; + car_y[4] = car_y[0]; + + // Plot car body + std::map keywords; + keywords["color"] = color; + plt::plot(car_x, car_y, keywords); + + // Plot base point + std::vector base_x = {x}; + std::vector base_y = {y}; + keywords["color"] = "red"; + keywords["marker"] = "o"; + plt::plot(base_x, base_y, keywords); +} + +int main() { + // Problem parameters + int state_dim = 4; // [x y theta v] + int control_dim = 2; // [steering_angle acceleration] + int horizon = 500; // Same as CDDP example + double timestep = 0.03; + double wheelbase = 2.0; + + // Initial and goal states + Eigen::VectorXd initial_state(state_dim); + initial_state << 1.0, 1.0, 1.5*M_PI, 0.0; + + Eigen::VectorXd goal_state(state_dim); + goal_state << 0.0, 0.0, 0.0, 0.0; + + // Define variables + casadi::MX x = casadi::MX::sym("x", state_dim); + casadi::MX u = casadi::MX::sym("u", control_dim); + + // Car dynamics + casadi::MX theta = x(2); + casadi::MX v = x(3); + casadi::MX delta = u(0); + casadi::MX a = u(1); + + // Continuous dynamics + casadi::MX dx = casadi::MX::zeros(state_dim); + using casadi::cos; + using casadi::sin; + using casadi::tan; + + dx(0) = v * cos(theta); + dx(1) = v * sin(theta); + dx(2) = v * tan(delta) / wheelbase; + dx(3) = a; + + // Discrete dynamics using Euler integration + casadi::MX f = x + timestep * dx; + casadi::Function F("F", {x, u}, {f}); + + // Decision variables + int n_states = (horizon + 1) * state_dim; + int n_controls = horizon * control_dim; + casadi::MX X = casadi::MX::sym("X", n_states); + casadi::MX U = casadi::MX::sym("U", n_controls); + casadi::MX z = casadi::MX::vertcat({X, U}); + + // Objective function terms + auto running_cost = [&](casadi::MX x, casadi::MX u) { + casadi::MX u_cost = 1e-2 * u(0) * u(0) + 1e-4 * u(1) * u(1); + casadi::MX xy_norm = x(0) * x(0) + x(1) * x(1); + casadi::MX x_cost = 1e-3 * (casadi::MX::sqrt(xy_norm/0.01 + 1) * 0.1 - 0.1); + return u_cost + x_cost; + }; + + auto terminal_cost = [&](casadi::MX x) { + casadi::MX cost = 0; + casadi::MX d = x - casadi::DM(std::vector(goal_state.data(), goal_state.data() + state_dim)); + casadi::MX xy_err = d(0) * d(0) + d(1) * d(1); + cost += 0.1 * (casadi::MX::sqrt(xy_err/0.01 + 1) * 0.1 - 0.1); + cost += 1.0 * (casadi::MX::sqrt(d(2)*d(2)/0.01 + 1) * 0.1 - 0.1); + cost += 0.3 * (casadi::MX::sqrt(d(3)*d(3)/1.0 + 1) * 1.0 - 1.0); + return cost; + }; + + // Build objective + casadi::MX J = 0; + for (int t = 0; t < horizon; t++) { + casadi::MX x_t = X(casadi::Slice(t*state_dim, (t+1)*state_dim)); + casadi::MX u_t = U(casadi::Slice(t*control_dim, (t+1)*control_dim)); + J += running_cost(x_t, u_t); + } + J += terminal_cost(X(casadi::Slice(horizon*state_dim, (horizon+1)*state_dim))); + + // Constraints + casadi::MX g; + + // Initial condition + g = casadi::MX::vertcat({g, X(casadi::Slice(0, state_dim)) - + casadi::DM(std::vector(initial_state.data(), initial_state.data() + state_dim))}); + + // Dynamics constraints + for (int t = 0; t < horizon; t++) { + casadi::MX x_t = X(casadi::Slice(t*state_dim, (t+1)*state_dim)); + casadi::MX u_t = U(casadi::Slice(t*control_dim, (t+1)*control_dim)); + casadi::MX x_next = F(casadi::MXVector{x_t, u_t})[0]; + casadi::MX x_next_sym = X(casadi::Slice((t+1)*state_dim, (t+2)*state_dim)); + g = casadi::MX::vertcat({g, x_next_sym - x_next}); + } + + // NLP + casadi::MXDict nlp = { + {"x", z}, + {"f", J}, + {"g", g} + }; + + // Solver options + casadi::Dict solver_opts; + solver_opts["ipopt.max_iter"] = 1000; + solver_opts["ipopt.print_level"] = 5; + solver_opts["print_time"] = true; + solver_opts["ipopt.tol"] = 1e-6; + + casadi::Function solver = casadi::nlpsol("solver", "ipopt", nlp, solver_opts); + + // Bounds + std::vector lbx(n_states + n_controls); + std::vector ubx(n_states + n_controls); + std::vector lbg(g.size1()); + std::vector ubg(g.size1()); + + // State bounds + for (int t = 0; t <= horizon; t++) { + for (int i = 0; i < state_dim; i++) { + lbx[t*state_dim + i] = -1e20; + ubx[t*state_dim + i] = 1e20; + } + } + + // Control bounds + for (int t = 0; t < horizon; t++) { + // Steering angle bounds + lbx[n_states + t*control_dim] = -0.5; + ubx[n_states + t*control_dim] = 0.5; + // Acceleration bounds + lbx[n_states + t*control_dim + 1] = -2.0; + ubx[n_states + t*control_dim + 1] = 2.0; + } + + // Constraint bounds (all equality constraints) + for (int i = 0; i < g.size1(); i++) { + lbg[i] = 0; + ubg[i] = 0; + } + + // Initial guess + std::vector x0(n_states + n_controls, 0.0); + + // Initial state + for (int i = 0; i < state_dim; i++) { + x0[i] = initial_state(i); + } + + // Linear interpolation for states + for (int t = 1; t <= horizon; t++) { + for (int i = 0; i < state_dim; i++) { + x0[t*state_dim + i] = initial_state(i) + + (goal_state(i) - initial_state(i)) * t / horizon; + } + } + + // Call the solver + auto start_time = std::chrono::high_resolution_clock::now(); + casadi::DMDict res = solver(casadi::DMDict{ + {"x0", x0}, + {"lbx", lbx}, + {"ubx", ubx}, + {"lbg", lbg}, + {"ubg", ubg} + }); + auto end_time = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end_time - start_time); + + std::cout << "Solve time: " << duration.count() << " microseconds" << std::endl; + + // EXIT: Optimal Solution Found. + // solver : t_proc (avg) t_wall (avg) n_eval + // nlp_f | 23.27ms (217.46us) 23.44ms (219.05us) 107 + // nlp_g | 27.51ms (257.09us) 27.57ms (257.65us) 107 + // nlp_grad_f | 39.72ms (374.68us) 39.87ms (376.14us) 106 + // nlp_hess_l | 244.66ms ( 2.35ms) 245.20ms ( 2.36ms) 104 + // nlp_jac_g | 176.39ms ( 1.66ms) 176.76ms ( 1.67ms) 106 + // total | 1.49 s ( 1.49 s) 1.49 s ( 1.49 s) 1 + // Solve time: 1488881 microseconds + + // Extract solution + std::vector sol = std::vector(res.at("x")); + + // Convert to state and control trajectories + std::vector X_sol(horizon + 1, Eigen::VectorXd(state_dim)); + std::vector U_sol(horizon, Eigen::VectorXd(control_dim)); + + for (int t = 0; t <= horizon; t++) { + for (int i = 0; i < state_dim; i++) { + X_sol[t](i) = sol[t*state_dim + i]; + } + } + + for (int t = 0; t < horizon; t++) { + for (int i = 0; i < control_dim; i++) { + U_sol[t](i) = sol[n_states + t*control_dim + i]; + } + } + + // Animation setup + Animation::AnimationConfig config; + config.width = 800; + config.height = 800; + config.frame_skip = 5; + config.frame_delay = 10; + Animation animation(config); + + // Car dimensions + double car_length = 2.1; + double car_width = 0.9; + + // Extract trajectory + std::vector x_hist, y_hist; + for(const auto& x : X_sol) { + x_hist.push_back(x(0)); + y_hist.push_back(x(1)); + } + + // Create animation frames + Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim); + for(size_t i = 0; i < X_sol.size(); i++) { + animation.newFrame(); + + plt::plot(x_hist, y_hist, "b-"); + plotCarBox(goal_state, empty_control, car_length, car_width, "r"); + plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k"); + + plt::grid(true); + plt::xlim(-4, 4); + plt::ylim(-4, 4); + + animation.saveFrame(i); + } + + animation.createGif("car_parking_ipopt.gif"); + + return 0; +} \ No newline at end of file diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index 34eeb01..0f4520a 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -25,6 +25,7 @@ #include "cddp_core/dynamical_system.hpp" #include "cddp_core/objective.hpp" #include "cddp_core/constraint.hpp" +#include "cddp_core/barrier.hpp" #include "cddp_core/cddp_core.hpp" #include "cddp_core/helper.hpp" #include "cddp_core/boxqp.hpp" diff --git a/include/cddp-cpp/cddp_core/barrier.hpp b/include/cddp-cpp/cddp_core/barrier.hpp new file mode 100644 index 0000000..892300c --- /dev/null +++ b/include/cddp-cpp/cddp_core/barrier.hpp @@ -0,0 +1,125 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#ifndef CDDP_BARRIER_HPP +#define CDDP_BARRIER_HPP + +#include +#include + +namespace cddp { + +/** + * @class LogBarrier + * @brief Implements a log-barrier function for inequality constraints of the form + * lower_bound <= g(x,u) <= upper_bound (element-wise). + */ +class LogBarrier { +public: + /** + * @brief Construct a log barrier with given parameters. + * + * @param barrier_coeff Coefficient controlling barrier steepness. + * @param relaxation_coeff Relaxation factor for numerical stability + * @param barrier_order Order of barrier polynomial + */ + LogBarrier(double barrier_coeff = 1e-2, + double relaxation_coeff = 1.0, + int barrier_order = 2, + bool is_relaxed_log_barrier = false); + + /** + * @brief Evaluate the barrier function for a given constraint. + * + * @param constraint A reference to the constraint object whose output is bounded. + * @param state Current state vector. + * @param control Current control vector. + * @return Barrier function value, or \f$+\infty\f$ if infeasible. + */ + double evaluate(const Constraint& constraint, + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const; + + /** + * @brief Compute the gradient (first derivative) w.r.t. state and control. + * + * @param constraint Constraint being enforced. + * @param state Current state vector. + * @param control Current control vector. + * @param barrier_value (Optional) The last evaluated barrier cost + * @return A tuple of two vectors: `(dBarrier/dx, dBarrier/du)`. + */ + std::tuple getGradients( + const Constraint& constraint, + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double barrier_value) const; + + /** + * @brief Compute second derivatives (Hessians) of the barrier function. + * + * @param constraint Constraint being enforced. + * @param state Current state vector. + * @param control Current control vector. + * @param barrier_value (Optional) The last evaluated barrier cost + * @return A tuple of matrices `(Hxx, Huu, Hxu)` in that order. + */ + std::tuple getHessians( + const Constraint& constraint, + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double barrier_value) const; + /** + * @brief Get the barrier coefficient. + * @return The coefficient controlling the steepness of the log penalty. + */ + double getBarrierCoeff() const { + return barrier_coeff_; + } + + /** + * @brief Set the barrier coefficient. + * @param barrier_coeff New log-barrier penalty coefficient. + */ + void setBarrierCoeff(double barrier_coeff) { + barrier_coeff_ = barrier_coeff; + } + + /** + * @brief Get the current relaxation coefficient. + * @return relaxation_coeff + */ + double getRelaxationCoeff() const { + return relaxation_coeff_; + } + + /** + * @brief Set the relaxation coefficient. + * @param relaxation_coeff + */ + void setRelaxationCoeff(double relaxation_coeff) { + relaxation_coeff_ = relaxation_coeff; + } + +private: + double barrier_coeff_; ///< Coefficient controlling barrier steepness + double relaxation_coeff_; ///< Relaxation factor for numerical stability + int barrier_order_; ///< Order of barrier polynomial + bool is_relaxed_log_barrier_; ///< Use relaxed log-barrier method +}; + +} // namespace cddp + +#endif // CDDP_BARRIER_HPP \ No newline at end of file diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index 079d60c..0f170b4 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -17,6 +17,7 @@ #define CDDP_CDDP_CORE_HPP #include // For std::cout, std::cerr +#include // For std::string #include // For std::unique_ptr #include // For std::map` #include // For std::setw @@ -30,12 +31,13 @@ #include "cddp_core/dynamical_system.hpp" #include "cddp_core/objective.hpp" #include "cddp_core/constraint.hpp" +#include "cddp_core/barrier.hpp" #include "cddp_core/boxqp.hpp" namespace cddp { struct CDDPOptions { - double cost_tolerance = 1e-7; // Tolerance for changes in cost function + double cost_tolerance = 1e-6; // Tolerance for changes in cost function double grad_tolerance = 1e-4; // Tolerance for cost gradient magnitude int max_iterations = 1; // Maximum number of iterations double max_cpu_time = 0.0; // Maximum CPU time for the solver in seconds @@ -50,24 +52,26 @@ struct CDDPOptions { // log-barrier method double barrier_coeff = 1e-2; // Coefficient for log-barrier method double barrier_factor = 0.90; // Factor for log-barrier method - double barrier_tolerance = 1e-6; // Tolerance for log-barrier method + double barrier_tolerance = 1e-8; // Tolerance for log-barrier method double relaxation_coeff = 1.0; // Relaxation for log-barrier method int barrier_order = 2; // Order for log-barrier method + double filter_acceptance = 1e-8; // Small value for filter acceptance + double constraint_tolerance = 1e-12; // Tolerance for constraint violation // Regularization options std::string regularization_type = "control"; // different regularization types: ["none", "control", "state", "both"] double regularization_state = 1e-6; // Regularization for state double regularization_state_step = 1.0; // Regularization step for state - double regularization_state_max = 1e10; // Maximum regularization - double regularization_state_min = 1e-6; // Minimum regularization - double regularization_state_factor = 1.6; // Factor for state regularization + double regularization_state_max = 1e4; // Maximum regularization + double regularization_state_min = 1e-8; // Minimum regularization + double regularization_state_factor = 1e1; // Factor for state regularization double regularization_control = 1e-6; // Regularization for control double regularization_control_step = 1.0; // Regularization step for control - double regularization_control_max = 1e10; // Maximum regularization - double regularization_control_min = 1e-6; // Minimum regularization - double regularization_control_factor = 1.6; // Factor for control regularization + double regularization_control_max = 1e4; // Maximum regularization + double regularization_control_min = 1e-8; // Minimum regularization + double regularization_control_factor = 1e1; // Factor for control regularization // Other options bool verbose = true; // Option for debug printing @@ -75,6 +79,8 @@ 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 + bool is_relaxed_log_barrier = false; // Use relaxed log-barrier method + bool early_termination = true; // Terminate early if cost does not change NOTE: This may be critical for some problems // Boxqp options double boxqp_max_iterations = 100; // Maximum number of iterations for boxqp @@ -104,8 +110,17 @@ struct ForwardPassResult { std::vector control_sequence; double cost; double lagrangian; - double alpha; - bool success; + double alpha = 1.0; + bool success = false; + double constraint_violation = 0.0; +}; + +struct FilterPoint { + double cost; + double violation; + bool dominates(const FilterPoint& other) const { + return cost <= other.cost && violation <= other.violation; + } }; class CDDP { @@ -131,8 +146,6 @@ class CDDP { const CDDPOptions& getOptions() const { return options_; } // Setters - // void setDynamicalSystem(std::unique_ptr system) { torch_system_ = std::move(system); } - /** * @brief Set the Dynamical System object * @param system Dynamical system object (unique_ptr) @@ -202,8 +215,6 @@ class CDDP { // Get a specific constraint by name template T* getConstraint(const std::string& name) const { - // 'find' is a const operation on standard associative containers - // and does not modify 'constraint_set_' auto it = constraint_set_.find(name); // For other constraints, return nullptr if not found @@ -230,21 +241,25 @@ class CDDP { void initializeCDDP(); // Solve the problem - CDDPSolution solve(); - CDDPSolution solveLogCDDP(); - CDDPSolution solveASCDDP(); + CDDPSolution solve(std::string solver_type = "CLCDDP"); + private: // Solver methods - ForwardPassResult solveForwardPass(double alpha); - bool solveBackwardPass(); + CDDPSolution solveCLCDDP(); + ForwardPassResult solveCLCDDPForwardPass(double alpha); + bool solveCLCDDPBackwardPass(); + CDDPSolution solveLogCDDP(); ForwardPassResult solveLogCDDPForwardPass(double alpha); bool solveLogCDDPBackwardPass(); + CDDPSolution solveASCDDP(); ForwardPassResult solveASCDDPForwardPass(double alpha); bool solveASCDDPBackwardPass(); // Helper methods + double computeConstraintViolation(const std::vector& X, const std::vector& U) const; + bool checkConvergence(double J_new, double J_old, double dJ, double expected_dV, double gradient_norm); void printSolverInfo(); @@ -253,7 +268,7 @@ class CDDP { void printIteration(int iter, double cost, double lagrangian, double grad_norm, double lambda_state, - double lambda_control, double alpha); + double lambda_control, double alpha, double mu, double constraint_violation); void printSolution(const CDDPSolution& solution); @@ -286,6 +301,11 @@ class CDDP { double alpha_; // Line search step size std::vector alphas_; + // Log-barrier + double mu_; // Barrier coefficient + double constraint_violation_; // Current constraint violation measure + double gamma_; // Small value for filter acceptance + // Feedforward and feedback gains std::vector k_; std::vector K_; diff --git a/include/cddp-cpp/cddp_core/constraint.hpp b/include/cddp-cpp/cddp_core/constraint.hpp index 65ad12c..419846b 100644 --- a/include/cddp-cpp/cddp_core/constraint.hpp +++ b/include/cddp-cpp/cddp_core/constraint.hpp @@ -1,7 +1,27 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + #ifndef CDDP_CONSTRAINT_HPP #define CDDP_CONSTRAINT_HPP #include +#include +#include +#include +#include namespace cddp { @@ -14,7 +34,8 @@ class Constraint { const std::string& getName() const { return name_; } // Evaluate the constraint function: g(x, u) - virtual Eigen::VectorXd evaluate(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0; + virtual Eigen::VectorXd evaluate(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const = 0; // Get the lower bound of the constraint virtual Eigen::VectorXd getLowerBound() const = 0; @@ -23,27 +44,52 @@ class Constraint { virtual Eigen::VectorXd getUpperBound() const = 0; // Get the Jacobian of the constraint w.r.t the state: dg/dx - virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0; + virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const = 0; // Get the Jacobian of the constraint w.r.t the control: dg/du - virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0; + virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const = 0; + + // Utility: Get both Jacobians: dg/dx, dg/du + virtual std::tuple + getJacobians(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const + { + return {getStateJacobian(state, control), + getControlJacobian(state, control)}; + } + + // Compute how far the constraint is violated + virtual double computeViolation(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const = 0; - // Get both Jacobians: dg/dx, dg/du - virtual std::tuple getJacobians(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return {getStateJacobian(state, control), getControlJacobian(state, control)}; + // Given g(x,u), compute violation from that vector + virtual double computeViolationFromValue(const Eigen::VectorXd& g) const = 0; + + // Used for constraints with a center (e.g., ball constraint) + virtual Eigen::VectorXd getCenter() const { + throw std::logic_error("This constraint type does not have a center."); } + private: std::string name_; // Name of the constraint }; +//------------------------------------------------------------------------------ class ControlBoxConstraint : public Constraint { public: - ControlBoxConstraint(const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound) - : Constraint("ControlBoxConstraint"), lower_bound_(lower_bound), upper_bound_(upper_bound) {} - - Eigen::VectorXd evaluate(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - return control; // The constraint is directly on the control + ControlBoxConstraint(const Eigen::VectorXd& lower_bound, + const Eigen::VectorXd& upper_bound) + : Constraint("ControlBoxConstraint"), + lower_bound_(lower_bound), + upper_bound_(upper_bound) {} + + Eigen::VectorXd evaluate(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return control; } Eigen::VectorXd getLowerBound() const override { @@ -54,11 +100,15 @@ class ControlBoxConstraint : public Constraint { return upper_bound_; } - Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - return Eigen::MatrixXd::Zero(control.size(), state.size()); + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return Eigen::MatrixXd::Zero(control.size(), control.size()); } - Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { return Eigen::MatrixXd::Identity(control.size(), control.size()); } @@ -66,6 +116,19 @@ class ControlBoxConstraint : public Constraint { return control.cwiseMax(lower_bound_).cwiseMin(upper_bound_); } + double computeViolation(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + Eigen::VectorXd g = evaluate(state, control); + return computeViolationFromValue(g); + } + + double computeViolationFromValue(const Eigen::VectorXd& g) const override { + // Sum of amounts by which g is above upper_bound or below lower_bound + return (g - upper_bound_).cwiseMax(0.0).sum() + + (lower_bound_ - g).cwiseMax(0.0).sum(); + } + private: Eigen::VectorXd lower_bound_; Eigen::VectorXd upper_bound_; @@ -73,11 +136,16 @@ class ControlBoxConstraint : public Constraint { class StateBoxConstraint : public Constraint { public: - StateBoxConstraint(const Eigen::VectorXd& lower_bound, const Eigen::VectorXd& upper_bound) - : Constraint("StateBoxConstraint"), lower_bound_(lower_bound), upper_bound_(upper_bound) {} - - Eigen::VectorXd evaluate(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - return state; // The constraint is directly on the state + StateBoxConstraint(const Eigen::VectorXd& lower_bound, + const Eigen::VectorXd& upper_bound) + : Constraint("StateBoxConstraint"), + lower_bound_(lower_bound), + upper_bound_(upper_bound) {} + + Eigen::VectorXd evaluate(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return state; } Eigen::VectorXd getLowerBound() const override { @@ -88,172 +156,215 @@ class StateBoxConstraint : public Constraint { return upper_bound_; } - Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { return Eigen::MatrixXd::Identity(state.size(), state.size()); } - Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { return Eigen::MatrixXd::Zero(state.size(), control.size()); } + Eigen::VectorXd clamp(const Eigen::VectorXd& state) const { + return state.cwiseMax(lower_bound_).cwiseMin(upper_bound_); + } + + double computeViolation(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + Eigen::VectorXd g = evaluate(state, control); + return computeViolationFromValue(g); + } + + double computeViolationFromValue(const Eigen::VectorXd& g) const override { + // Same logic as ControlBoxConstraint but for the state + return (g - upper_bound_).cwiseMax(0.0).sum() + + (lower_bound_ - g).cwiseMax(0.0).sum(); + } + private: Eigen::VectorXd lower_bound_; Eigen::VectorXd upper_bound_; }; -// CircleConstraint (assuming the circle is centered at the origin) -class CircleConstraint : public Constraint { +class LinearConstraint : public Constraint { public: - CircleConstraint(double radius) : Constraint("CircleConstraint"), radius_(radius) {} - - Eigen::VectorXd evaluate(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - Eigen::Vector2d position(state(0), state(1)); // Assuming the first two elements of the state are x and y position - return Eigen::VectorXd::Constant(1, position.squaredNorm()); + LinearConstraint(const Eigen::MatrixXd& A, + const Eigen::VectorXd& b, + double scale_factor = 1.0) + : Constraint("LinearConstraint"), + A_(A), + b_(b), + scale_factor_(scale_factor) {} + + Eigen::VectorXd evaluate(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return A_ * state; } Eigen::VectorXd getLowerBound() const override { - return Eigen::VectorXd::Constant(1, 0.0); + return Eigen::VectorXd::Constant(A_.rows(), -std::numeric_limits::infinity()); } Eigen::VectorXd getUpperBound() const override { - return Eigen::VectorXd::Constant(1, radius_ * radius_); + return b_; } - Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - Eigen::MatrixXd jacobian(1, state.size()); - jacobian << 2 * state(0), 2 * state(1), Eigen::RowVectorXd::Zero(state.size() - 2); // Assuming x and y are the first two state elements - return jacobian; + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return A_; } - Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { - return Eigen::MatrixXd::Zero(1, control.size()); + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return Eigen::MatrixXd::Zero(A_.rows(), control.size()); + } + + double computeViolation(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + Eigen::VectorXd g = evaluate(state, control); + return computeViolationFromValue(g); + } + + double computeViolationFromValue(const Eigen::VectorXd& g) const override { + return std::max(0.0, (b_ - g).maxCoeff()); } private: - double radius_; + Eigen::MatrixXd A_; + Eigen::VectorXd b_; + double scale_factor_; }; -/** -* @brief Log barrier function for constrained optimization -* -* Implements a log barrier interior point method to handle inequality constraints. -*/ -class LogBarrier { + +class BallConstraint : public Constraint { public: - /** - * @brief Types of bounds to consider in barrier function - */ - enum BoundType { - UPPER_BOUND, ///< Only consider upper bounds - LOWER_BOUND, ///< Only consider lower bounds - BOTH_BOUNDS ///< Consider both upper and lower bounds - }; - - /** - * @brief Construct a log barrier with given parameters - * @param barrier_coeff Coefficient controlling barrier steepness - * @param relaxation_coeff Relaxation factor for numerical stability - * @param barrier_order Order of barrier polynomial (1=linear, 2=quadratic) - * @param bound_type Which bounds to include in barrier - */ - LogBarrier(double barrier_coeff = 1e-2, double relaxation_coeff = 1.0, - int barrier_order = 2); - - /** - * @brief Evaluate barrier function for given constraint - * @param constraint Constraint being enforced - * @param state Current state - * @param control Current control input - * @return Barrier function value - */ - double evaluate(const Constraint& constraint, - const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const; - - /** - * @brief Get state and control gradients of barrier - * @param constraint Constraint being enforced - * @param state Current state - * @param control Current control input - * @param barrier_value Value of barrier function - * @return Tuple of state and control gradients - */ - std::tuple getGradients( - const Constraint& constraint, - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double barrier_value) const; - - /** - * @brief Get Hessians of barrier function - * @param constraint Constraint being enforced - * @param state Current state - * @param control Current control input - * @param barrier_value Value of barrier function - * @return Tuple of state, control and cross Hessians - */ - std::tuple getHessians( - const Constraint& constraint, - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double barrier_value) const; - - /** - * @brief Get current barrier coefficient - * @return Current barrier coefficient - */ - double getBarrierCoeff() { - return barrier_coeff_; - } - - /** - * @brief Set barrier coefficient to new value - * @param barrier_coeff New barrier coefficient value - */ - void setBarrierCoeff(double barrier_coeff) { - barrier_coeff_ = barrier_coeff; - } - - /** - * @brief Get current relaxation coefficient - * @return Current relaxation coefficient - */ - double getRelaxationCoeff() { - return relaxation_coeff_; - } - - /** - * @brief Set relaxation coefficient to new value - * @param relaxation_coeff New relaxation coefficient value - */ - void setRelaxationCoeff(double relaxation_coeff) { - relaxation_coeff_ = relaxation_coeff; - } - - /** - * @brief Get current barrier order - * @return Current barrier order - */ - BoundType getBoundType() { - return bound_type_; - } - - /** - * @brief Set barrier order to new value - * @param bound_type New bound type - */ - void setBoundType(BoundType bound_type) { - bound_type_ = bound_type; + BallConstraint(double radius, + const Eigen::VectorXd& center, + double scale_factor = 1.0) + : Constraint("BallConstraint"), + radius_(radius), + center_(center), + scale_factor_(scale_factor) + { + } + + + Eigen::VectorXd evaluate(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + const Eigen::VectorXd& diff = state - center_; + return Eigen::VectorXd::Constant(1, scale_factor_ * diff.squaredNorm()); + } + + Eigen::VectorXd getLowerBound() const override { + return Eigen::VectorXd::Constant(1, radius_ * radius_); + } + + Eigen::VectorXd getUpperBound() const override { + return Eigen::VectorXd::Constant(1, std::numeric_limits::infinity()); } + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + const Eigen::VectorXd& diff = state - center_; + Eigen::MatrixXd jac(1, state.size()); + jac.row(0) = (2.0 * scale_factor_) * diff.transpose(); + return jac; + } + + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + return Eigen::MatrixXd::Zero(1, control.size()); + } + + + double computeViolation(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override + { + Eigen::VectorXd g = evaluate(state, control); + return computeViolationFromValue(g); + } + + double computeViolationFromValue(const Eigen::VectorXd& g) const override + { + double val = g(0); + double lb = getLowerBound()(0); + return std::max(0.0, val - lb); + } + + Eigen::VectorXd getCenter() const { return center_; } + private: - double barrier_coeff_; ///< Coefficient controlling barrier steepness - double relaxation_coeff_; ///< Relaxation factor for numerical stability - int barrier_order_; ///< Order of barrier polynomial - BoundType bound_type_; ///< Which bounds to include + double radius_; + Eigen::VectorXd center_; + double scale_factor_; }; +// class QuadraticConstraint : public Constraint { +// public: +// QuadraticConstraint(const Eigen::MatrixXd& Q, +// const Eigen::VectorXd& q, +// double r, +// double scale_factor = 1.0) +// : Constraint("QuadraticConstraint"), +// Q_(Q), +// q_(q), +// r_(r), +// scale_factor_(scale_factor) {} + +// Eigen::VectorXd evaluate(const Eigen::VectorXd& state, +// const Eigen::VectorXd& control) const override +// { +// return 0.5 * state.transpose() * Q_ * state + q_.transpose() * state + r_; +// } + +// Eigen::VectorXd getLowerBound() const override { +// return Eigen::VectorXd::Constant(1, -std::numeric_limits::infinity()); +// } + +// Eigen::VectorXd getUpperBound() const override { +// return Eigen::VectorXd::Zero(1); +// } + +// Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, +// const Eigen::VectorXd& control) const override +// { +// return Q_ * state + q_; +// } + +// Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, +// const Eigen::VectorXd& control) const override +// { +// return Eigen::MatrixXd::Zero(1, control.size()); +// } + +// double computeViolation(const Eigen::VectorXd& state, +// const Eigen::VectorXd& control) const override +// { +// Eigen::VectorXd g = evaluate(state, control); +// return computeViolationFromValue(g); +// } + +// double computeViolationFromValue(const Eigen::VectorXd& g) const override { +// return std::max(0.0, g(0)); +// } +// private: +// Eigen::MatrixXd Q_; +// Eigen::VectorXd q_; +// double r_; +// double scale_factor_; +// }; } // namespace cddp -#endif // CDDP_CONSTRAINT_HPP \ No newline at end of file +#endif // CDDP_CONSTRAINT_HPP diff --git a/results/animations/car_parking_ipopt.gif b/results/animations/car_parking_ipopt.gif new file mode 100644 index 0000000..1cf9ff4 Binary files /dev/null and b/results/animations/car_parking_ipopt.gif differ diff --git a/results/tests/dubincs_car_logcddp_test.png b/results/tests/dubincs_car_logcddp_test.png index 1a25da1..924ba73 100644 Binary files a/results/tests/dubincs_car_logcddp_test.png and b/results/tests/dubincs_car_logcddp_test.png differ diff --git a/src/cddp_core/asddp_core.cpp b/src/cddp_core/asddp_core.cpp index bf3e0de..bb61c24 100644 --- a/src/cddp_core/asddp_core.cpp +++ b/src/cddp_core/asddp_core.cpp @@ -84,7 +84,7 @@ CDDPSolution CDDP::solveASCDDP() solution.lagrangian_sequence.push_back(L_); if (options_.verbose) { - printIteration(0, J_, J_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information + printIteration(0, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); // Initial iteration information } // Start timer @@ -120,34 +120,29 @@ CDDPSolution CDDP::solveASCDDP() std::cerr << "CDDP: Backward pass failed" << std::endl; // 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_min); - 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 (options_.regularization_type == "state") { + 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_min); + } else if (options_.regularization_type == "control") { + 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); + } else if (options_.regularization_type == "both") { + 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_min); + 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: Backward pass 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); - 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_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); - - if (regularization_state_ < 1e-5 && regularization_control_ < 1e-5) { - solution.converged = true; - break; - } - - } - // 2. Forward pass (either single-threaded or multi-threaded) ForwardPassResult best_result; best_result.cost = std::numeric_limits::infinity(); @@ -189,10 +184,30 @@ CDDPSolution CDDP::solveASCDDP() 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); - regularization_control_step_ = std::min(regularization_control_step_ / options_.regularization_control_factor, 1 / options_.regularization_control_factor); - regularization_control_ *= regularization_control_step_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); + if (options_.regularization_type == "state") { + 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_ = options_.regularization_state_min; + } + } else if (options_.regularization_type == "control") { + 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_ = options_.regularization_control_min; + } + } else if (options_.regularization_type == "both") { + 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_ = options_.regularization_state_min; + } + 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_ = options_.regularization_control_min; + } + } // Check termination if (dJ_ < options_.cost_tolerance) { @@ -200,23 +215,68 @@ CDDPSolution CDDP::solveASCDDP() break; } } else { + bool early_termination_flag = false; // TODO: Improve early termination // 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); + if (options_.regularization_type == "state") { + if (regularization_state_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_state_ = std::min(regularization_state_ * regularization_state_step_, options_.regularization_state_max); + + } else if (options_.regularization_type == "control") { + if (regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + regularization_control_ = std::min(regularization_control_ * regularization_control_step_, options_.regularization_control_max); + } else if (options_.regularization_type == "both") { + if (regularization_state_ < 1e-2 || + regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + } else { + early_termination_flag = true; + } + // Check early termination + if (options_.early_termination && early_termination_flag) { + if (dJ_ < options_.cost_tolerance * 1e2 || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + if (options_.verbose) { + std::cerr << "CDDP: Early termination due to small cost reduction" << std::endl; + } + break; + } + } + // 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; + if ((dJ_ < options_.cost_tolerance * 1e2) || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + } else + { + // We are forced to large regularization but still not near local min + solution.converged = false; + } + if (options_.verbose) { + std::cerr << "CDDP: Regularization limit reached. " + << (solution.converged ? "Treating as converged." : "Not converged.") + << std::endl; + } break; } } // Print iteration information if (options_.verbose) { - printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); } } diff --git a/src/cddp_core/barrier.cpp b/src/cddp_core/barrier.cpp new file mode 100644 index 0000000..aced617 --- /dev/null +++ b/src/cddp_core/barrier.cpp @@ -0,0 +1,151 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include "cddp_core/constraint.hpp" +#include "cddp_core/barrier.hpp" +#include + +namespace cddp +{ + +LogBarrier::LogBarrier(double barrier_coeff, double relaxation_coeff, int barrier_order, bool is_relaxed_log_barrier) + : barrier_coeff_(barrier_coeff), relaxation_coeff_(relaxation_coeff), barrier_order_(barrier_order), is_relaxed_log_barrier_(is_relaxed_log_barrier) {} + +double LogBarrier::evaluate(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control) const +{ + const double eps = barrier_coeff_ * 1e2; // Small value to avoid log(0) + double barrier_cost = 0.0; + if (constraint.getName() == "ControlBoxConstraint" || + constraint.getName() == "StateBoxConstraint") + { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); + const Eigen::VectorXd& lower_bound = constraint.getLowerBound(); + const Eigen::VectorXd& upper_bound = constraint.getUpperBound(); + + Eigen::VectorXd upper = -upper_bound + constraint_value; + Eigen::VectorXd lower = -constraint_value + lower_bound; + + // Apply bounds to avoid log(0) + upper = upper.array().max(eps); + lower = lower.array().max(eps); + + const Eigen::VectorXd& upper_log = upper.array().log(); + const Eigen::VectorXd& lower_log = lower.array().log(); + + barrier_cost = barrier_coeff_ * (upper_log.sum() + lower_log.sum()); + + } else if (constraint.getName() == "BallConstraint") { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); // ||y - c||^2 + const Eigen::VectorXd& lower = constraint.getLowerBound(); // r^2 + double z = -lower(0) + constraint_value(0); // -r^2 + ||y - c||^2 > 0 + double log_z = log(std::max(z, eps)); + barrier_cost = -barrier_coeff_ * log_z; + } else { + std::cerr << "LogBarrier: Unsupported constraint type" << std::endl; + } + return barrier_cost; +} + +std::tuple LogBarrier::getGradients(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control, double barrier_value) const +{ + const double eps = barrier_coeff_ * 1e2; // Small value to avoid log(0) + Eigen::VectorXd state_grad = Eigen::VectorXd::Zero(state.size()); + Eigen::VectorXd control_grad = Eigen::VectorXd::Zero(control.size()); + if (constraint.getName() == "ControlBoxConstraint" || + constraint.getName() == "StateBoxConstraint") + { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); + const Eigen::VectorXd& lower_bound = constraint.getLowerBound(); + const Eigen::VectorXd& upper_bound = constraint.getUpperBound(); + + Eigen::VectorXd upper_diff = (upper_bound - constraint_value).unaryExpr([eps](double x) { + return std::max(x, eps); + }); + Eigen::VectorXd lower_diff = (constraint_value - lower_bound).unaryExpr([eps](double x) { + return std::max(x, eps); + }); + + // Then compute gradient using array operations + Eigen::VectorXd gradient = barrier_coeff_ * ( + upper_diff.array().inverse() - lower_diff.array().inverse() + ).matrix(); + + if (constraint.getName() == "ControlBoxConstraint") { + control_grad = gradient; + } else { // StateBoxConstraint + state_grad = gradient; + } + } else if (constraint.getName() == "BallConstraint") { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); // ||y - c||^2 + const Eigen::VectorXd& diff = state - constraint.getCenter(); // y - c + const Eigen::VectorXd& lower = constraint.getLowerBound(); // r^2 + double z = -lower(0) + constraint_value(0); // -r^2 + ||y - c||^2 > 0 + z = std::max(z, eps); + state_grad = -barrier_coeff_ * 2.0 * diff / z; + + } else{ + std::cerr << "LogBarrier: Unsupported constraint type" << std::endl; + } + + return {state_grad, control_grad}; +} + +std::tuple LogBarrier::getHessians(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control, double barrier_value) const +{ + const double eps = barrier_coeff_ * 1e2; // Small value to avoid log(0) + Eigen::MatrixXd state_hess = Eigen::MatrixXd::Zero(state.size(), state.size()); + Eigen::MatrixXd control_hess = Eigen::MatrixXd::Zero(control.size(), control.size()); + Eigen::MatrixXd cross_hess = Eigen::MatrixXd::Zero(control.size(), state.size()); + + if (constraint.getName() == "ControlBoxConstraint" || + constraint.getName() == "StateBoxConstraint") + { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); + const Eigen::VectorXd& lower_bound = constraint.getLowerBound(); + const Eigen::VectorXd& upper_bound = constraint.getUpperBound(); + + Eigen::VectorXd upper_diff = (upper_bound - constraint_value).unaryExpr([eps](double x) { + return std::max(x, eps); + }); + Eigen::VectorXd lower_diff = (constraint_value - lower_bound).unaryExpr([eps](double x) { + return std::max(x, eps); + }); + + // Compute squared reciprocals using array operations + Eigen::VectorXd hess_diag = barrier_coeff_ * ( + upper_diff.array().square().inverse() + + lower_diff.array().square().inverse() + ).matrix(); + + if (constraint.getName() == "ControlBoxConstraint") { + control_hess = hess_diag.asDiagonal(); + } else { // StateBoxConstraint + state_hess = hess_diag.asDiagonal(); + } + } else if (constraint.getName() == "BallConstraint") { + const Eigen::VectorXd& constraint_value = constraint.evaluate(state, control); // ||y - c||^2 + const Eigen::VectorXd& diff = state - constraint.getCenter(); // y - c + const Eigen::VectorXd& lower = constraint.getLowerBound(); // r^2 + double z = -lower(0) + constraint_value(0); // -r^2 + ||y - c||^2 > 0 + z = std::max(z, eps); + Eigen::MatrixXd hess = 4 * barrier_coeff_ * diff * diff.transpose() / (z * z) - 2 * barrier_coeff_ * Eigen::MatrixXd::Identity(state.size(), state.size()) / z; + state_hess = hess; + } else { + std::cerr << "LogBarrier: Unsupported constraint type" << std::endl; + } + + return {state_hess, control_hess, cross_hess}; +} +} // namespace cddp \ No newline at end of file diff --git a/src/cddp_core/cddp_core.cpp b/src/cddp_core/cddp_core.cpp index 4a0269f..1045517 100644 --- a/src/cddp_core/cddp_core.cpp +++ b/src/cddp_core/cddp_core.cpp @@ -15,20 +15,17 @@ */ #include // For std::cout, std::cerr -#include // For std::setw -#include // For std::unique_ptr -#include // For std::map -#include // For std::log #include -#include // For timing -#include // For parallel execution policies +#include +#include +#include // For std::unique_ptr +#include // For std::map #include "cddp_core/cddp_core.hpp" #include "cddp_core/boxqp.hpp" namespace cddp { - // Constructor CDDP::CDDP(const Eigen::VectorXd &initial_state, const Eigen::VectorXd &reference_state, @@ -53,6 +50,19 @@ CDDP::CDDP(const Eigen::VectorXd &initial_state, } } +cddp::CDDPSolution CDDP::solve(std::string solver_type) { + if (solver_type == "CLCDDP" || solver_type == "CLDDP") { + return solveCLCDDP(); + } else if (solver_type == "LogCDDP" || solver_type == "LogDDP") { + return solveLogCDDP(); + } else if (solver_type == "ASCDDP" || solver_type == "ASDDP") { + return solveASCDDP(); + } else { + std::cerr << "CDDP::solve: Unknown solver type" << std::endl; + throw std::runtime_error("CDDP::solve: Unknown solver type"); + } +} + void CDDP::setInitialTrajectory(const std::vector &X, const std::vector &U) { if (!system_) { @@ -180,7 +190,11 @@ void CDDP::initializeCDDP() } // Initialize Log-barrier object - log_barrier_ = std::make_unique(options_.barrier_coeff, options_.relaxation_coeff, options_.barrier_order); + log_barrier_ = std::make_unique(options_.barrier_coeff, options_.relaxation_coeff, options_.barrier_order, options_.is_relaxed_log_barrier); + mu_ = options_.barrier_coeff; + log_barrier_->setBarrierCoeff(mu_); + gamma_ = options_.filter_acceptance; + constraint_violation_ = 0.0; // Initialize boxqp options boxqp_options_.max_iterations = options_.boxqp_max_iterations; @@ -196,424 +210,15 @@ void CDDP::initializeCDDP() initialized_ = true; } - -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"); - } - - // 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 - solution.lagrangian_sequence.reserve(options_.max_iterations); // Reserve space for efficiency - solution.cost_sequence.push_back(J_); - solution.lagrangian_sequence.push_back(L_); - - if (options_.verbose) { - printIteration(0, J_, J_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information - } - - // Start timer - auto start_time = std::chrono::high_resolution_clock::now(); - int iter = 0; - - // Main loop of CDDP - while (iter < options_.max_iterations) - { - ++iter; - solution.iterations = iter; - - // Check maximum CPU time - 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) { - if (options_.verbose) { - std::cerr << "CDDP: Maximum CPU time reached. Returning current solution" << std::endl; - } - break; - } +double CDDP::computeConstraintViolation(const std::vector& X, + const std::vector& U) const { + double total_violation = 0.0; + for (const auto& constraint : constraint_set_) { + for (size_t t = 0; t < U.size(); ++t) { + total_violation += constraint.second->computeViolation(X[t], U[t]); } - - // 1. Backward pass: Solve Riccati recursion to compute optimal control law - bool backward_pass_success = false; - while (!backward_pass_success) { - backward_pass_success = solveBackwardPass(); - - if (!backward_pass_success) { - std::cerr << "CDDP: Backward pass failed" << std::endl; - - // 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_min); - 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; - } - 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); - 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_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); - - if (regularization_state_ < 1e-5 && regularization_control_ < 1e-5) { - 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; - - 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; - } - } - } - - // 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_ * (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_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); - - // Check termination - if (dJ_ < options_.cost_tolerance) { - solution.converged = true; - 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.state_sequence = X_; - solution.control_sequence = U_; - solution.feedback_gain = K_; - solution.alpha = alpha_; - solution.solve_time = duration.count(); // Time in microseconds - - if (options_.verbose) { - printSolution(solution); } - - return solution; -} - -bool CDDP::solveBackwardPass() { - // Initialize variables - const int state_dim = system_->getStateDim(); - const int control_dim = system_->getControlDim(); - - // Extract control box constraint - auto control_box_constraint = getConstraint("ControlBoxConstraint"); - - // 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); - Eigen::MatrixXd B(state_dim, control_dim); - Eigen::VectorXd Q_x(state_dim); - 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::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) { - // Get state and control - const Eigen::VectorXd& x = X_[t]; - const Eigen::VectorXd& u = U_[t]; - - // TODO: Precompute Jacobians and store them? - // 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; - B = timestep_ * Fu; - - // 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); - - // 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; - } 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_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; - } - return false; - } - - if (control_box_constraint == nullptr) { - const Eigen::MatrixXd &H = Q_uu_reg.inverse(); - k = -H * Q_u; - 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; - } - - // 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); - } - } - - // 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 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 = 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()); - - // TODO: Add constraint optimality gap analysis - optimality_gap_ = Qu_error; - } - - if (options_.debug) { - std::cout << "Qu_error: " << Qu_error << std::endl; - std::cout << "dV: " << dV_.transpose() << std::endl; - } - - return true; -} - -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(); - - // 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; - - if (control_box_constraint != nullptr) { - U_new[t] = control_box_constraint->clamp(U_new[t]); - } - - 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; + return total_violation; } void CDDP::printSolverInfo() @@ -657,6 +262,9 @@ void CDDP::printOptions(const CDDPOptions &options) 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 << " Barrier Order: " << std::setw(5) << options.barrier_order << "\n"; + std::cout << " Filter Acceptance: " << std::setw(5) << options.filter_acceptance << "\n"; + std::cout << " Constraint Tolerance: " << std::setw(5) << options.constraint_tolerance << "\n"; std::cout << "\nRegularization:\n"; std::cout << " Regularization Type: " << options.regularization_type << "\n"; @@ -677,36 +285,50 @@ void CDDP::printOptions(const CDDPOptions &options) 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 << " Relaxed Log-Barrier: " << (options.is_relaxed_log_barrier ? "Yes" : "No") << "\n"; + std::cout << " Early Termination: " << (options.early_termination ? "Yes" : "No") << "\n"; + + std::cout << "\nBoxQP:\n"; + std::cout << " BoxQP Max Iterations: " << options.boxqp_max_iterations << "\n"; + std::cout << " BoxQP Min Grad: " << options.boxqp_min_grad << "\n"; + std::cout << " BoxQP Min Rel Improve: " << options.boxqp_min_rel_improve << "\n"; + std::cout << " BoxQP Step Dec: " << options.boxqp_step_dec << "\n"; + std::cout << " BoxQP Min Step: " << options.boxqp_min_step << "\n"; + std::cout << " BoxQP Armijo: " << options.boxqp_armijo << "\n"; + std::cout << " BoxQP Verbose: " << (options.boxqp_verbose ? "Yes" : "No") << "\n"; std::cout << "========================================\n\n"; } -void CDDP::printIteration(int iter, double cost, double lagrangian, double grad_norm, - double lambda_state, double lambda_control, double step_size) +void CDDP::printIteration(int iter, double cost, double lagrangian, double grad_norm, + double lambda_state, double lambda_control, double step_size, + double mu, double constraint_violation) { - // 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; + if (iter % 10 == 0) + { + std::cout << std::setw(5) << "Iter" + << std::setw(12) << "Cost" + << std::setw(12) << "Lagr" + << std::setw(10) << "Grad" + << std::setw(10) << "Step" + << std::setw(10) << "RegS" + << std::setw(10) << "RegC" + << std::setw(10) << "Mu" + << std::setw(10) << "Viol" + << std::endl; + std::cout << std::string(89, '-') << std::endl; + } + + std::cout << std::setw(5) << iter + << std::setw(12) << std::scientific << std::setprecision(3) << cost + << std::setw(12) << std::scientific << std::setprecision(3) << lagrangian + << std::setw(10) << std::scientific << std::setprecision(2) << grad_norm + << std::setw(10) << std::fixed << std::setprecision(3) << step_size + << std::setw(10) << std::scientific << std::setprecision(2) << lambda_state + << std::setw(10) << std::scientific << std::setprecision(2) << lambda_control + << std::setw(10) << std::scientific << std::setprecision(2) << mu + << std::setw(10) << std::scientific << std::setprecision(2) << constraint_violation + << std::endl; } void CDDP::printSolution(const CDDPSolution &solution) diff --git a/src/cddp_core/clddp_core.cpp b/src/cddp_core/clddp_core.cpp new file mode 100644 index 0000000..579eacd --- /dev/null +++ b/src/cddp_core/clddp_core.cpp @@ -0,0 +1,508 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#include // For std::cout, std::cerr +#include // For std::setw +#include // For std::unique_ptr +#include // For std::map +#include // For std::log +#include +#include // For timing +#include // For parallel execution policies + +#include "cddp_core/cddp_core.hpp" +#include "cddp_core/boxqp.hpp" + +namespace cddp +{ +CDDPSolution CDDP::solveCLCDDP() { + // Initialize if not done + if (!initialized_) { + initializeCDDP(); + } + + if (!initialized_) { + std::cerr << "CDDP: Initialization failed" << std::endl; + throw std::runtime_error("CDDP: Initialization failed"); + } + + // 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 + solution.lagrangian_sequence.reserve(options_.max_iterations); // Reserve space for efficiency + solution.cost_sequence.push_back(J_); + solution.lagrangian_sequence.push_back(L_); + + if (options_.verbose) { + printIteration(0, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); // Initial iteration information + } + + // Start timer + auto start_time = std::chrono::high_resolution_clock::now(); + int iter = 0; + + // Main loop of CDDP + while (iter < options_.max_iterations) + { + ++iter; + solution.iterations = iter; + + // Check maximum CPU time + 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) { + 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) { + backward_pass_success = solveCLCDDPBackwardPass(); + + if (!backward_pass_success) { + std::cerr << "CDDP: Backward pass failed" << std::endl; + + // Increase regularization + if (options_.regularization_type == "state") { + 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_min); + } else if (options_.regularization_type == "control") { + 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); + } else if (options_.regularization_type == "both") { + 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_min); + 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: Backward pass regularization limit reached" << std::endl; + } + break; // Exit if regularization limit reached + } + continue; // Continue if backward pass fails + } + } + + // 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; + + if (!options_.use_parallel) { + // Single-threaded execution with early termination + for (double alpha : alphas_) { + ForwardPassResult result = solveCLCDDPForwardPass(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 solveCLCDDPForwardPass(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; + } + } + } + + // 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 + if (options_.regularization_type == "state") { + 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_ = options_.regularization_state_min; + } + } else if (options_.regularization_type == "control") { + 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_ = options_.regularization_control_min; + } + } else if (options_.regularization_type == "both") { + 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_ = options_.regularization_state_min; + } + 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_ = options_.regularization_control_min; + } + } + + // Check termination + if (dJ_ < options_.cost_tolerance) { + solution.converged = true; + break; + } + } else { + bool early_termination_flag = false; // TODO: Improve early termination + // Increase regularization + if (options_.regularization_type == "state") { + if (regularization_state_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_state_ = std::min(regularization_state_ * regularization_state_step_, options_.regularization_state_max); + + } else if (options_.regularization_type == "control") { + if (regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + regularization_control_ = std::min(regularization_control_ * regularization_control_step_, options_.regularization_control_max); + } else if (options_.regularization_type == "both") { + if (regularization_state_ < 1e-2 || + regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + } else { + early_termination_flag = true; + } + + // Check early termination + if (options_.early_termination && early_termination_flag) { + if (dJ_ < options_.cost_tolerance * 1e2 || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + if (options_.verbose) { + std::cerr << "CDDP: Early termination due to small cost reduction" << std::endl; + } + break; + } + } + + // Check regularization limit + if (regularization_state_ >= options_.regularization_state_max || regularization_control_ >= options_.regularization_control_max) { + if ((dJ_ < options_.cost_tolerance * 1e2) || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + } else + { + // We are forced to large regularization but still not near local min + solution.converged = false; + } + if (options_.verbose) { + std::cerr << "CDDP: Regularization limit reached. " + << (solution.converged ? "Treating as converged." : "Not converged.") + << std::endl; + } + break; + } + } + + // Print iteration information + if (options_.verbose) { + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); + } + } + auto end_time = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end_time - start_time); + + // Finalize solution + solution.state_sequence = X_; + solution.control_sequence = U_; + solution.feedback_gain = K_; + solution.alpha = alpha_; + solution.solve_time = duration.count(); // Time in microseconds + + if (options_.verbose) { + printSolution(solution); + } + + return solution; +} + +bool CDDP::solveCLCDDPBackwardPass() { + // Initialize variables + const int state_dim = system_->getStateDim(); + const int control_dim = system_->getControlDim(); + + // Extract control box constraint + auto control_box_constraint = getConstraint("ControlBoxConstraint"); + + // 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); + Eigen::MatrixXd B(state_dim, control_dim); + Eigen::VectorXd Q_x(state_dim); + 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::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) { + // Get state and control + const Eigen::VectorXd& x = X_[t]; + const Eigen::VectorXd& u = U_[t]; + + // TODO: Precompute Jacobians and store them? + // 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; + B = timestep_ * Fu; + + // 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); + + // 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; + } 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_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; + } + return false; + } + + if (control_box_constraint == nullptr) { + const Eigen::MatrixXd &H = Q_uu_reg.inverse(); + k = -H * Q_u; + 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; + } + + // 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); + } + } + + // 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 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 = 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()); + + // TODO: Add constraint optimality gap analysis + optimality_gap_ = Qu_error; + } + + if (options_.debug) { + std::cout << "Qu_error: " << Qu_error << std::endl; + std::cout << "dV: " << dV_.transpose() << std::endl; + } + + return true; +} + +ForwardPassResult CDDP::solveCLCDDPForwardPass(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(); + + // 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; + + if (control_box_constraint != nullptr) { + U_new[t] = control_box_constraint->clamp(U_new[t]); + } + + 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; +} +} // namespace cddp diff --git a/src/cddp_core/constraint.cpp b/src/cddp_core/constraint.cpp index 4c86825..6f26c5d 100644 --- a/src/cddp_core/constraint.cpp +++ b/src/cddp_core/constraint.cpp @@ -1,156 +1,22 @@ -#include "cddp_core/constraint.hpp" -#include - -namespace cddp -{ - -LogBarrier::LogBarrier(double barrier_coeff, double relaxation_coeff, int barrier_order) - : barrier_coeff_(barrier_coeff), relaxation_coeff_(relaxation_coeff), barrier_order_(barrier_order) {} - -double LogBarrier::evaluate(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control) const -{ - Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - Eigen::VectorXd lower_bound = constraint.getLowerBound(); - Eigen::VectorXd upper_bound = constraint.getUpperBound(); - - double barrier_cost = 0.0; - for (int i = 0; i < constraint_value.size(); ++i) - { - if (constraint.getName() == "ControlBoxConstraint" || constraint.getName() == "StateBoxConstraint") - { - double upper = -upper_bound(i) + constraint_value(i); - double lower = -constraint_value(i) + lower_bound(i); - - if (upper > relaxation_coeff_) - { - barrier_cost -= barrier_coeff_ * std::log(upper); - } - else - { - barrier_cost += (barrier_order_ - 1) / barrier_order_ * (std::pow((upper - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_) - 1.0) - std::log(relaxation_coeff_); - } - - if (lower > relaxation_coeff_) - { - barrier_cost -= barrier_coeff_ * std::log(lower); - } - else - { - barrier_cost += (barrier_order_ - 1) / barrier_order_ * (std::pow((lower - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_) - 1.0) - std::log(relaxation_coeff_); - } - } - else - { - if (constraint_value(i) < lower_bound(i)) - { - return std::numeric_limits::infinity(); - } - else - { - barrier_cost -= barrier_coeff_ * std::log(lower_bound(i) - constraint_value(i)); - } - } - } - return barrier_cost; -} - -std::tuple LogBarrier::getGradients(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control, double barrier_value) const -{ - if (constraint.getName() == "ControlBoxConstraint" || constraint.getName() == "StateBoxConstraint") { - Eigen::VectorXd state_grad = Eigen::VectorXd::Zero(state.size()); - Eigen::VectorXd control_grad = Eigen::VectorXd::Zero(control.size()); +/* + Copyright 2024 Tomo Sasaki - Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - Eigen::VectorXd lower_bound = constraint.getLowerBound(); - Eigen::VectorXd upper_bound = constraint.getUpperBound(); + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at - for (int i = 0; i < constraint_value.size(); ++i) - { - double upper = upper_bound(i) - constraint_value(i); - double lower = constraint_value(i) - lower_bound(i); + https://www.apache.org/licenses/LICENSE-2.0 - if (upper > relaxation_coeff_) - { - state_grad(i) = -barrier_coeff_ / upper; - control_grad(i) = -barrier_coeff_ / upper; - } - else - { - state_grad(i) = (barrier_order_ - 1) / barrier_order_ * std::pow((upper - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 1) / relaxation_coeff_; - control_grad(i) = (barrier_order_ - 1) / barrier_order_ * std::pow((upper - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 1) / relaxation_coeff_; - } - - if (lower > relaxation_coeff_) - { - state_grad(i) = barrier_coeff_ / lower; - control_grad(i) = barrier_coeff_ / lower; - } - else - { - state_grad(i) = (barrier_order_ - 1) / barrier_order_ * std::pow((lower - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 1) / relaxation_coeff_; - control_grad(i) = (barrier_order_ - 1) / barrier_order_ * std::pow((lower - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 1) / relaxation_coeff_; - } - } - - return {state_grad, control_grad}; - } - Eigen::VectorXd state_grad = Eigen::VectorXd::Zero(state.size()); - Eigen::VectorXd control_grad = Eigen::VectorXd::Zero(control.size()); - - // Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - // Eigen::VectorXd lower_bound = constraint.getLowerBound(); - // Eigen::VectorXd upper_bound = constraint.getUpperBound(); - - // for (int i = 0; i < constraint_value.size(); ++i) - // { - // double upper = upper_bound(i) - constraint_value(i); - // double lower = constraint_value(i) - lower_bound(i); - // } - - return {state_grad, control_grad}; -} + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include "cddp_core/constraint.hpp" +#include -std::tuple LogBarrier::getHessians(const Constraint &constraint, const Eigen::VectorXd &state, const Eigen::VectorXd &control, double barrier_value) const +namespace cddp { - Eigen::MatrixXd state_hess = Eigen::MatrixXd::Zero(state.size(), state.size()); - Eigen::MatrixXd control_hess = Eigen::MatrixXd::Zero(control.size(), control.size()); - Eigen::MatrixXd cross_hess = Eigen::MatrixXd::Zero(control.size(), state.size()); - - Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - Eigen::VectorXd lower_bound = constraint.getLowerBound(); - Eigen::VectorXd upper_bound = constraint.getUpperBound(); - - if (constraint.getName() == "ControlBoxConstraint" || constraint.getName() == "StateBoxConstraint") - { - for (int i = 0; i < constraint_value.size(); ++i) - { - double upper = upper_bound(i) - constraint_value(i); - double lower = constraint_value(i) - lower_bound(i); - - if (upper > relaxation_coeff_) - { - state_hess(i, i) = barrier_coeff_ / std::pow(upper, 2); - control_hess(i, i) = barrier_coeff_ / std::pow(upper, 2); - } - else - { - state_hess(i, i) = (barrier_order_ - 1) / barrier_order_ * (barrier_order_ - 1) / barrier_order_ * std::pow((upper - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 2) / std::pow(relaxation_coeff_, 2); - control_hess(i, i) = (barrier_order_ - 1) / barrier_order_ * (barrier_order_ - 1) / barrier_order_ * std::pow((upper - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 2) / std::pow(relaxation_coeff_, 2); - } - - if (lower > relaxation_coeff_) - { - state_hess(i, i) = -barrier_coeff_ / std::pow(lower, 2); - control_hess(i, i) = -barrier_coeff_ / std::pow(lower, 2); - } - else - { - state_hess(i, i) = (barrier_order_ - 1) / barrier_order_ * (barrier_order_ - 1) / barrier_order_ * std::pow((lower - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 2) / std::pow(relaxation_coeff_, 2); - control_hess(i, i) = (barrier_order_ - 1) / barrier_order_ * (barrier_order_ - 1) / barrier_order_ * std::pow((lower - barrier_order_ * relaxation_coeff_) / relaxation_coeff_ * (barrier_order_ - 1), barrier_order_ - 2) / std::pow(relaxation_coeff_, 2); - } - } - } - return {state_hess, control_hess, cross_hess}; -} } // namespace cddp \ No newline at end of file diff --git a/src/cddp_core/logddp_core.cpp b/src/cddp_core/logddp_core.cpp index eee48b9..05eb726 100644 --- a/src/cddp_core/logddp_core.cpp +++ b/src/cddp_core/logddp_core.cpp @@ -44,6 +44,10 @@ CDDPSolution CDDP::solveLogCDDP() throw std::runtime_error("CDDP: Initialization failed"); } + // Initialize log barrier parameters + mu_ = options_.barrier_coeff; // Initial barrier coefficient + log_barrier_->setBarrierCoeff(mu_); + // Prepare solution struct CDDPSolution solution; solution.converged = false; @@ -77,7 +81,7 @@ CDDPSolution CDDP::solveLogCDDP() if (options_.verbose) { - printIteration(0, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); // Initial iteration information + printIteration(0, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); // Initial iteration information } // Start timer @@ -110,14 +114,22 @@ CDDPSolution CDDP::solveLogCDDP() std::cerr << "CDDP: Backward pass failed" << std::endl; // 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_min); - 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 (options_.regularization_type == "state") { + 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_min); + } else if (options_.regularization_type == "control") { + 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); + } else if (options_.regularization_type == "both") { + 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_min); + 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; + std::cerr << "CDDP: Backward pass regularization limit reached" << std::endl; } break; // Exit if regularization limit reached } @@ -125,19 +137,6 @@ CDDPSolution CDDP::solveLogCDDP() } } - // 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); - 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_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); - - if (regularization_state_ < 1e-5 && regularization_control_ < 1e-5) { - solution.converged = true; - break; - } - } - // 2. Forward pass (either single-threaded or multi-threaded) ForwardPassResult best_result; best_result.cost = std::numeric_limits::infinity(); @@ -206,14 +205,35 @@ CDDPSolution CDDP::solveLogCDDP() 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_ * (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_ * (regularization_control_ > options_.regularization_control_min ? 1.0 : 0.0); + if (options_.regularization_type == "state") { + 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_ = options_.regularization_state_min; + } + } else if (options_.regularization_type == "control") { + 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_ = options_.regularization_control_min; + } + } else if (options_.regularization_type == "both") { + 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_ = options_.regularization_state_min; + } + 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_ = options_.regularization_control_min; + } + } // Check termination if (dJ_ < options_.cost_tolerance) { @@ -221,24 +241,81 @@ CDDPSolution CDDP::solveLogCDDP() break; } } else { + bool early_termination_flag = false; // TODO: Improve early termination // 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); + if (options_.regularization_type == "state") { + if (regularization_state_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_state_ = std::min(regularization_state_ * regularization_state_step_, options_.regularization_state_max); + + } else if (options_.regularization_type == "control") { + if (regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_control_step_ = std::max(regularization_control_step_ * options_.regularization_control_factor, options_.regularization_control_factor); + regularization_control_ = std::min(regularization_control_ * regularization_control_step_, options_.regularization_control_max); + } else if (options_.regularization_type == "both") { + if (regularization_state_ < 1e-2 || + regularization_control_ < 1e-2) { + early_termination_flag = true; // Early termination if regularization is fairly small + } + regularization_state_step_ = std::max(regularization_state_step_ * options_.regularization_state_factor, options_.regularization_state_factor); + regularization_state_ = std::min(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::min(regularization_control_ * regularization_control_step_, options_.regularization_control_max); + } else { + early_termination_flag = true; + } + // Check early termination + if (options_.early_termination && early_termination_flag) { + if (dJ_ < options_.cost_tolerance * 1e2 || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + if (options_.verbose) { + std::cerr << "CDDP: Early termination due to small cost reduction" << std::endl; + } + break; + } + } + // 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; + if ((dJ_ < options_.cost_tolerance * 1e2) || + (optimality_gap_ < options_.grad_tolerance * 1e1)) + { + solution.converged = true; + } else + { + // We are forced to large regularization but still not near local min + solution.converged = false; + } + if (options_.verbose) { + std::cerr << "CDDP: Regularization limit reached. " + << (solution.converged ? "Treating as converged." : "Not converged.") + << std::endl; + } break; } } // Print iteration information if (options_.verbose) { - printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_); + printIteration(iter, J_, L_, optimality_gap_, regularization_state_, regularization_control_, alpha_, mu_, constraint_violation_); + } + + // Update barrier parameter mu + if (forward_pass_success && optimality_gap_ < options_.grad_tolerance && mu_ > options_.barrier_tolerance) { + // Dramatically decrease mu if optimization is going well + mu_ = std::max(mu_ * 0.1, options_.barrier_tolerance); + } else { + // Normal decrease rate + mu_ = std::max(mu_ * options_.barrier_factor, options_.barrier_tolerance); } + log_barrier_->setBarrierCoeff(mu_); } auto end_time = std::chrono::high_resolution_clock::now(); auto duration = std::chrono::duration_cast(end_time - start_time); @@ -311,6 +388,19 @@ bool CDDP::solveLogCDDPBackwardPass() { Q_ux = l_ux + B.transpose() * V_xx * A; Q_uu = l_uu + B.transpose() * V_xx * B; + // Apply Log-barrier cost gradients and Hessians + for (const auto &constraint : constraint_set_) + { + auto [L_x, L_u] = log_barrier_->getGradients(*constraint.second, x, u, 0.0); + Q_x += L_x; + Q_u += L_u; + + auto [L_xx, L_uu, L_ux] = log_barrier_->getHessians(*constraint.second, x, u, 0.0); + Q_xx += L_xx; + Q_uu += L_uu; + Q_ux += L_ux; + } + // TODO: Apply Cholesky decomposition to Q_uu later? // // Symmetrize Q_uu for Cholesky decomposition // Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); @@ -337,57 +427,10 @@ bool CDDP::solveLogCDDPBackwardPass() { return false; } - if (control_box_constraint == nullptr) { - const Eigen::MatrixXd &H = Q_uu_reg.inverse(); - k = -H * Q_u; - 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; - } - - // 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); - } - } - - // 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); - } - } - } - + const Eigen::MatrixXd &H = Q_uu_reg.inverse(); + k = -H * Q_u; + K = -H * Q_ux_reg; + // Store feedforward and feedback gain k_[t] = k; K_[t] = K; @@ -431,9 +474,13 @@ ForwardPassResult CDDP::solveLogCDDPForwardPass(double alpha) { std::vector U_new = U_; X_new[0] = initial_state_; double J_new = 0.0, L_new = 0.0; + double total_violation = 0.0; auto control_box_constraint = getConstraint("ControlBoxConstraint"); + // Current filter point + FilterPoint current{J_, computeConstraintViolation(X_, U_)}; + for (int t = 0; t < horizon_; ++t) { const Eigen::VectorXd& x = X_new[t]; const Eigen::VectorXd& u = U_new[t]; @@ -448,24 +495,54 @@ ForwardPassResult CDDP::solveLogCDDPForwardPass(double alpha) { const double cost = objective_->running_cost(x, U_new[t], t); const double barrier_cost = log_barrier_->evaluate(*control_box_constraint, x, u); + + J_new += cost; L_new += (cost + barrier_cost); X_new[t + 1] = system_->getDiscreteDynamics(x, U_new[t]); + + for (const auto& constraint : constraint_set_) { + if (constraint.first == "ControlBoxConstraint") { + continue; + } + total_violation += constraint.second->computeViolation(X_new[t+1], U_new[t]); + + // Early termination if constraint is violated + if (total_violation > options_.constraint_tolerance && !options_.is_relaxed_log_barrier) { + if (options_.debug) { + std::cerr << "CDDP: Constraint violation at time " << t << std::endl; + std::cerr << "Constraint violation: " << total_violation << std::endl; + std::cout << "state: " << X_new[t+1].transpose() << std::endl; + std::cout << "control: " << U_new[t].transpose() << std::endl; + std::cout << "alpha: " << alpha << std::endl; + } + return result; // Return with failure + } + } } 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; - 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 = L_new; - - return result; + + FilterPoint candidate{J_new, total_violation}; + + // Filter acceptance criteria + bool sufficient_progress = (J_new < J_ - gamma_ * total_violation) || + (total_violation < (1 - gamma_) * current.violation); + + bool acceptable = sufficient_progress && !current.dominates(candidate); + + if (acceptable) { + double expected = -alpha * (dV_(0) + 0.5 * alpha * dV_(1)); + double reduction_ratio = expected > 0.0 ? (J_ - J_new) / expected : + std::copysign(1.0, J_ - J_new); + + result.success = acceptable; + result.state_sequence = X_new; + result.control_sequence = U_new; + result.cost = J_new; + result.lagrangian = L_new; + } + + return result; } } // namespace cddp \ No newline at end of file diff --git a/tests/cddp_core/test_asddp_core.cpp b/tests/cddp_core/test_asddp_core.cpp index 109d97e..1887a45 100644 --- a/tests/cddp_core/test_asddp_core.cpp +++ b/tests/cddp_core/test_asddp_core.cpp @@ -90,7 +90,7 @@ TEST(CDDPTest, SolveASCDDP) { cddp_solver.setInitialTrajectory(X, U); // // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solveASCDDP(); + cddp::CDDPSolution solution = cddp_solver.solve("ASCDDP"); ASSERT_TRUE(solution.converged); diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index 4e4bc01..a73c4d2 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -37,12 +37,12 @@ TEST(CDDPTest, Solve) { std::unique_ptr system = std::make_unique(timestep, integration_type); // Create unique_ptr // Create objective function - Eigen::MatrixXd Q = 5 * Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim); Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); - Qf << 1200.0, 0.0, 0.0, - 0.0, 1200.0, 0.0, - 0.0, 0.0, 700.0; + Qf << 50.0, 0.0, 0.0, + 0.0, 50.0, 0.0, + 0.0, 0.0, 10.0; Qf = 0.5 * Qf; Eigen::VectorXd goal_state(state_dim); goal_state << 2.0, 2.0, M_PI/2.0; @@ -78,9 +78,9 @@ TEST(CDDPTest, Solve) { // Define constraints Eigen::VectorXd control_lower_bound(control_dim); - control_lower_bound << -1.0, -3.1415; + control_lower_bound << -1.0, -M_PI; Eigen::VectorXd control_upper_bound(control_dim); - control_upper_bound << 1.0, 3.1415; + control_upper_bound << 1.0, M_PI; // Add the constraint to the solver cddp_solver.addConstraint(std::string("ControlBoxConstraint"), std::make_unique(control_lower_bound, control_upper_bound)); diff --git a/tests/cddp_core/test_constraint.cpp b/tests/cddp_core/test_constraint.cpp index fe9817c..63db6c7 100644 --- a/tests/cddp_core/test_constraint.cpp +++ b/tests/cddp_core/test_constraint.cpp @@ -40,9 +40,31 @@ TEST(ControlBoxConstraintTest, Evaluate) { ASSERT_TRUE(constraint_value.isApprox(control)); } +TEST(StateBoxConstraint, Evaluate) { + // Create a constraint with lower and upper bounds + Eigen::VectorXd lower_bound(2); + lower_bound << -1.0, -2.0; + Eigen::VectorXd upper_bound(2); + upper_bound << 1.0, 2.0; + cddp::StateBoxConstraint constraint(lower_bound, upper_bound); + + // Test with a state within the bounds + Eigen::VectorXd state(2); + state << 0.5, 1.0; + Eigen::VectorXd control(2); // Control doesn't matter for this constraint + control << 0.0; + Eigen::VectorXd constraint_value = constraint.evaluate(state, control); + ASSERT_TRUE(constraint_value.isApprox(state)); + + // Test with a state outside the bounds + state << 1.5, -2.5; + constraint_value = constraint.evaluate(state, control); + ASSERT_TRUE(constraint_value.isApprox(state)); +} + TEST(CircleConstraintTest, Evaluate) { // Create a circle constraint with a radius of 2.0 - cddp::CircleConstraint constraint(2.0); + cddp::BallConstraint constraint(2.0, Eigen::Vector2d(0.0, 0.0)); // Test with a state inside the circle Eigen::VectorXd state(2); @@ -50,10 +72,54 @@ TEST(CircleConstraintTest, Evaluate) { Eigen::VectorXd control(1); // Control doesn't matter for this constraint control << 0.0; Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - ASSERT_NEAR(constraint_value(0), 2.0, 1e-6); // 1.0^2 + 1.0^2 = 2.0 + ASSERT_NEAR(constraint_value(0), 2.0, 1e-6); // Test with a state outside the circle state << 2.5, 1.5; constraint_value = constraint.evaluate(state, control); - ASSERT_NEAR(constraint_value(0), 6.25 + 2.25, 1e-6); // 2.5^2 + 1.5^2 = 8.5 + ASSERT_NEAR(constraint_value(0), 8.5, 1e-6); +} + +TEST(CircleConstraintTest, Gradients) { + // Create a circle constraint with a radius of 2.0 + cddp::BallConstraint constraint(2.0, Eigen::Vector2d(0.0, 0.0)); + + // Test with a state inside the circle + Eigen::VectorXd state(2); + state << 1.0, 1.0; + Eigen::VectorXd control(1); // Control doesn't matter for this constraint + control << 0.0; + Eigen::VectorXd constraint_value = constraint.evaluate(state, control); + auto gradients = constraint.getJacobians(state, control); + ASSERT_TRUE(std::get<0>(gradients).isApprox(Eigen::Vector2d(2.0, 2.0))); + ASSERT_TRUE(std::get<1>(gradients).isApprox(Eigen::Vector2d(0.0, 0.0))); + + // Test with a state outside the circle + state << 2.5, 1.5; + constraint_value = constraint.evaluate(state, control); + gradients = constraint.getJacobians(state, control); + ASSERT_TRUE(std::get<0>(gradients).isApprox(Eigen::Vector2d(5.0, 3.0))); + ASSERT_TRUE(std::get<1>(gradients).isApprox(Eigen::Vector2d(0.0, 0.0))); +} + +TEST(LinearConstraintTest, Evaluate) { + // Create a linear constraint with A = [1, 1], b = 1 + Eigen::MatrixXd A(1, 2); + A << 1.0, 1.0; + Eigen::VectorXd b(1); + b << 1.0; + cddp::LinearConstraint constraint(A, b); + + // Test with a state that satisfies the constraint + Eigen::VectorXd state(2); + state << 0.5, 0.5; + Eigen::VectorXd control(1); // Control doesn't matter for this constraint + control << 0.0; + Eigen::VectorXd constraint_value = constraint.evaluate(state, control); + ASSERT_NEAR(constraint_value(0), 1.0, 1e-6); + + // Test with a state that violates the constraint + state << 0.5, -0.5; + constraint_value = constraint.evaluate(state, control); + ASSERT_NEAR(constraint_value(0), 0.0, 1e-6); } \ No newline at end of file diff --git a/tests/cddp_core/test_logcddp_core.cpp b/tests/cddp_core/test_logcddp_core.cpp index 131337c..c865919 100644 --- a/tests/cddp_core/test_logcddp_core.cpp +++ b/tests/cddp_core/test_logcddp_core.cpp @@ -37,7 +37,8 @@ TEST(CDDPTest, SolveLogCDDP) { std::unique_ptr system = std::make_unique(timestep, integration_type); // Create unique_ptr // Create objective function - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd Q = 0.1 * Eigen::MatrixXd::Identity(state_dim, state_dim); + Q(2, 2) = 0.0; Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim); Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); Qf << 50.0, 0.0, 0.0, @@ -55,10 +56,19 @@ TEST(CDDPTest, SolveLogCDDP) { Eigen::VectorXd initial_state(state_dim); initial_state << 0.0, 0.0, M_PI/4.0; - // Create CDDP options + // Create CDDP Options cddp::CDDPOptions options; - options.max_iterations = 10; - options.cost_tolerance = 1e-1; + options.max_iterations = 30; + options.cost_tolerance = 1e-5; + options.use_parallel = true; + options.num_threads = 10; + options.regularization_type = "both"; + options.regularization_state = 1e-5; + options.regularization_control = 1e-3; + options.barrier_coeff = 1e-2; + options.barrier_factor = 1e-1; + options.verbose = true; + options.debug = false; // Create CDDP solver cddp::CDDP cddp_solver( @@ -72,15 +82,26 @@ TEST(CDDPTest, SolveLogCDDP) { cddp_solver.setDynamicalSystem(std::move(system)); cddp_solver.setObjective(std::move(objective)); - // Define constraints + // Define control box constraints Eigen::VectorXd control_lower_bound(control_dim); control_lower_bound << -1.0, -M_PI; Eigen::VectorXd control_upper_bound(control_dim); control_upper_bound << 1.0, M_PI; - - // 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"); + auto control_box_constraint = cddp_solver.getConstraint("ControlBoxConstraint"); + + // Define state box constraints + Eigen::VectorXd state_lower_bound(state_dim); + state_lower_bound << -0.1, -0.1, -10.0; + Eigen::VectorXd state_upper_bound(state_dim); + state_upper_bound << 2.5, 2.5, 10.0; + cddp_solver.addConstraint(std::string("StateBoxConstraint"), std::make_unique(state_lower_bound, state_upper_bound)); + auto state_box_constraint = cddp_solver.getConstraint("StateBoxConstraint"); + + // Define ball constraint + double radius = 0.2; + Eigen::Vector2d center(1.0, 1.0); + // cddp_solver.addConstraint(std::string("BallConstraint"), std::make_unique(radius, center, 0.1)); // Set options cddp_solver.setOptions(options); @@ -88,10 +109,24 @@ TEST(CDDPTest, SolveLogCDDP) { // Set initial trajectory std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); + std::vector x_arr0(horizon + 1, 0.0); + std::vector y_arr0(horizon + 1, 0.0); + X[0] = initial_state; + double v = 0.01; + for (int i = 0; i < horizon + 1; ++i) { + double x = X[i](0); + double y = X[i](1); + double theta = X[i](2); + + x_arr0[i] = X[i](0); + y_arr0[i] = X[i](1); + X[i+1] = Eigen::Vector3d(x + v * cos(theta), y + v * sin(theta), theta - 0.01); + } + cddp_solver.setInitialTrajectory(X, U); // Solve the problem - cddp::CDDPSolution solution = cddp_solver.solveLogCDDP(); + cddp::CDDPSolution solution = cddp_solver.solve("LogCDDP"); solution.converged = true; ASSERT_TRUE(solution.converged); @@ -124,6 +159,25 @@ TEST(CDDPTest, SolveLogCDDP) { // // Plot the solution by subplots // plt::subplot(2, 1, 1); // plt::plot(x_arr, y_arr); + // plt::plot(x_arr0, y_arr0, "r--"); + + // // // Plot circle constraint + // std::vector circle_x, circle_y; + // for (double theta = 0.0; theta < 2 * M_PI; theta += 0.01) { + // circle_x.push_back(center(0) + radius * cos(theta)); + // circle_y.push_back(center(1) + radius * sin(theta)); + // } + // plt::plot(circle_x, circle_y, "g--"); + + // // Plot boundaries + // std::vector x_lim1 = {0.0, 2.5}; + // std::vector x_lim2 = {2.5, 2.5}; + // std::vector y_lim1 = {2.5, 2.5}; + // std::vector y_lim2 = {0.0, 2.5}; + + // plt::plot(x_lim1, y_lim1, "r--"); + // plt::plot(x_lim2, y_lim2, "r--"); + // plt::title("State Trajectory"); // plt::xlabel("x"); // plt::ylabel("y");