From 7f6b36c7242626e536657c866df29a099668e8d6 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 31 Mar 2025 15:51:32 -0400 Subject: [PATCH] Added new line search step size parameters for control, slack, and dual variables and Cleaned up formatting and ensured consistent use of namespaces. --- include/cddp-cpp/cddp_core/cddp_core.hpp | 770 ++++++++++++----------- 1 file changed, 398 insertions(+), 372 deletions(-) diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index 077369d..3c19062 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -17,10 +17,10 @@ #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 +#include // For std::string +#include // For std::unique_ptr +#include // For std::map` +#include // For std::setw #include #include #include @@ -28,382 +28,408 @@ #include // #include "torch/torch.h" -#include "cddp_core/dynamical_system.hpp" +#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-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 - - // Line search method - int max_line_search_iterations = 11; // Maximum iterations for line search - double backtracking_coeff = 1.0; // Coefficient for line search backtracking - double backtracking_min = 1e-7; // Minimum step size for line search - double backtracking_factor = 0.5; // Factor for line search backtracking - double minimum_reduction_ratio = 1e-6; // Minimum reduction for line search - - // interior-point method - double mu_initial = 1e-2; // Initial barrier coefficient - double mu_min = 1e-8; // Minimum barrier coefficient - double mu_max = 1e1; // Maximum barrier coefficient - double mu_reduction_ratio = 0.1; // Factor for barrier coefficient - - // log-barrier method - double barrier_coeff = 1e-1; // Coefficient for log-barrier method - double barrier_factor = 0.10; // Factor 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 - - // ipddp options - double dual_scale = 1e-1; // Initial scale for dual variables - double slack_scale = 1e-2; // Initial scale for slack variables - - // 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 = 1e6; // 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 = 1e5; // 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 - bool debug = false; // Option for debug mode - bool header_and_footer = true; // Option for header and footer - 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 - double boxqp_min_grad = 1e-8; // Minimum gradient norm for boxqp - double boxqp_min_rel_improve = 1e-8; // Minimum relative improvement for boxqp - double boxqp_step_dec = 0.6; // Step decrease factor for boxqp - double boxqp_min_step = 1e-22; // Minimum step size for boxqp - double boxqp_armijo = 0.1; // Armijo parameter for boxqp - bool boxqp_verbose = false; // Print debug info for boxqp -}; - -struct CDDPSolution { - std::vector time_sequence; - std::vector control_sequence; - std::vector state_sequence; - std::vector dual_sequence; - std::vector slack_sequence; - std::vector cost_sequence; - std::vector lagrangian_sequence; - std::vector control_gain; - std::vector dual_gain; - std::vector slack_gain; - int iterations; - double alpha; - bool converged; - double solve_time; -}; - -struct ForwardPassResult { - std::vector state_sequence; - std::vector control_sequence; - std::map> dual_sequence; - std::map> slack_sequence; - std::map> constraint_sequence; - double cost; - double lagrangian; - double alpha = 1.0; - bool success = false; - double constraint_violation = 0.0; -}; - -struct FilterPoint { - double log_cost; - double violation; - - // Default constructor - FilterPoint() : log_cost(0.0), violation(0.0) {} - - // Constructor with parameters - FilterPoint(double lc, double v) : log_cost(lc), violation(v) {} - - bool dominates(const FilterPoint& other) const { - return log_cost <= other.log_cost && violation <= other.violation; - } -}; - -class CDDP { -public: - // Constructor - CDDP(const Eigen::VectorXd& initial_state, - const Eigen::VectorXd& reference_state, - int horizon, - double timestep, - std::unique_ptr system = nullptr, - std::unique_ptr objective = nullptr, - const CDDPOptions& options = CDDPOptions()); - - // Accessor methods - - // Getters - const DynamicalSystem& getSystem() const { return *system_; } - const Objective& getObjective() const { return *objective_; } - const Eigen::VectorXd& getInitialState() const { return initial_state_; } - const Eigen::VectorXd& getReferenceState() const { return reference_state_; } - int getHorizon() const { return horizon_; } - double getTimestep() const { return timestep_; } - int getStateDim() const { return system_->getStateDim(); } - int getControlDim() const { return system_->getControlDim(); } - int getTotalDualDim() const { return total_dual_dim_; } - const CDDPOptions& getOptions() const { return options_; } - - // Setters - /** - * @brief Set the Dynamical System object - * @param system Dynamical system object (unique_ptr) - */ - void setDynamicalSystem(std::unique_ptr system) { system_ = std::move(system); } - - /** - * @brief Set the Initial state - * @param initial_state Initial state - */ - void setInitialState(const Eigen::VectorXd& initial_state) { initial_state_ = initial_state; } - - /** - * @brief Set the Reference state - * @param reference_state Reference state - */ - void setReferenceState(const Eigen::VectorXd& reference_state) { - reference_state_ = reference_state; - // Update the objective reference state - objective_->setReferenceState(reference_state); - } - - void setReferenceStates(const std::vector& reference_states) { - reference_states_ = reference_states; - // Update the objective reference states - objective_->setReferenceStates(reference_states); - } - - /** - * @brief Set the time horizon for the problem - * @param horizon Time horizon - */ - void setHorizon(int horizon) { horizon_ = horizon; } - - /** - * @brief Set the time step for the problem - * @param timestep Time step - */ - void setTimestep(double timestep) { timestep_ = timestep; } - - /** - * @brief Set the options for the solver - * @param options Solver options - */ - void setOptions(const CDDPOptions& options) { options_ = options; } - - /** - * @brief Set the Objective function - * @param objective Objective function object (unique_ptr) - */ - void setObjective(std::unique_ptr objective) { objective_ = std::move(objective); } - - /** - * @brief Set the Initial Trajectory - * @param X state trajectory - * @param U control trajectory - */ - void setInitialTrajectory(const std::vector& X, const std::vector& U); - - /** - * @brief Add a constraint to the problem - * - * @param constraint_name constraint name given by the user - * @param constraint constraint object - */ - void addConstraint(std::string constraint_name, std::unique_ptr constraint) { - // Insert into the map - constraint_set_[constraint_name] = std::move(constraint); - - // Increment total_dual_dim_ - int dim = constraint_set_[constraint_name]->getDualDim(); - total_dual_dim_ += dim; - - initialized_ = false; // Reset the initialization flag - } - - /** - * @brief Get a specific constraint by name - * - * @tparam T Type of constraint - * @param name Name of the constraint - * @return T* Pointer to the constraint - */ - // Get a specific constraint by name - template - T* getConstraint(const std::string& name) const { - auto it = constraint_set_.find(name); - - // For other constraints, return nullptr if not found - if (it == constraint_set_.end()) { - return nullptr; +namespace cddp +{ + + struct CDDPOptions + { + 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 + + // Line search method + int max_line_search_iterations = 11; // Maximum iterations for line search + double backtracking_coeff = 1.0; // Coefficient for line search backtracking + double backtracking_min = 1e-7; // Minimum step size for line search + double backtracking_factor = 0.5; // Factor for line search backtracking + double minimum_reduction_ratio = 1e-6; // Minimum reduction for line search + + // interior-point method + double mu_initial = 1e-2; // Initial barrier coefficient + double mu_min = 1e-8; // Minimum barrier coefficient + double mu_max = 1e1; // Maximum barrier coefficient + double mu_reduction_ratio = 0.1; // Factor for barrier coefficient + + // log-barrier method + double barrier_coeff = 1e-1; // Coefficient for log-barrier method + double barrier_factor = 0.10; // Factor 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 + + // ipddp options + double dual_scale = 1e-1; // Initial scale for dual variables + double slack_scale = 1e-2; // Initial scale for slack variables + + // 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 = 1e6; // 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 = 1e5; // Maximum regularization + double regularization_control_min = 1e-8; // Minimum regularization + double regularization_control_factor = 1e1; // Factor for control regularization + + // Line search options for IPDDP + bool use_separate_line_search = false; // Use separate line search for control, slack, and dual variables + double control_step_size = 1.0; // Initial step size for control variables + double slack_step_size = 1.0; // Initial step size for slack variables + double dual_step_size = 1.0; // Initial step size for dual variables + + // Other options + bool verbose = true; // Option for debug printing + bool debug = false; // Option for debug mode + bool header_and_footer = true; // Option for header and footer + 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 + double boxqp_min_grad = 1e-8; // Minimum gradient norm for boxqp + double boxqp_min_rel_improve = 1e-8; // Minimum relative improvement for boxqp + double boxqp_step_dec = 0.6; // Step decrease factor for boxqp + double boxqp_min_step = 1e-22; // Minimum step size for boxqp + double boxqp_armijo = 0.1; // Armijo parameter for boxqp + bool boxqp_verbose = false; // Print debug info for boxqp + }; + + struct CDDPSolution + { + std::vector time_sequence; + std::vector control_sequence; + std::vector state_sequence; + std::vector dual_sequence; + std::vector slack_sequence; + std::vector cost_sequence; + std::vector lagrangian_sequence; + std::vector control_gain; + std::vector dual_gain; + std::vector slack_gain; + int iterations; + double alpha; + bool converged; + double solve_time; + }; + + struct ForwardPassResult + { + std::vector state_sequence; + std::vector control_sequence; + std::map> dual_sequence; + std::map> slack_sequence; + std::map> constraint_sequence; + double cost; + double lagrangian; + double alpha = 1.0; // Common alpha or primary alpha for control + double alpha_slack = 1.0; // Step size for slack variables + double alpha_dual = 1.0; // Step size for dual variables + bool success = false; + double constraint_violation = 0.0; + }; + + struct FilterPoint + { + double log_cost; + double violation; + + // Default constructor + FilterPoint() : log_cost(0.0), violation(0.0) {} + + // Constructor with parameters + FilterPoint(double lc, double v) : log_cost(lc), violation(v) {} + + bool dominates(const FilterPoint &other) const + { + return log_cost <= other.log_cost && violation <= other.violation; + } + }; + + class CDDP + { + public: + // Constructor + CDDP(const Eigen::VectorXd &initial_state, + const Eigen::VectorXd &reference_state, + int horizon, + double timestep, + std::unique_ptr system = nullptr, + std::unique_ptr objective = nullptr, + const CDDPOptions &options = CDDPOptions()); + + // Accessor methods + + // Getters + const DynamicalSystem &getSystem() const { return *system_; } + const Objective &getObjective() const { return *objective_; } + const Eigen::VectorXd &getInitialState() const { return initial_state_; } + const Eigen::VectorXd &getReferenceState() const { return reference_state_; } + int getHorizon() const { return horizon_; } + double getTimestep() const { return timestep_; } + int getStateDim() const { return system_->getStateDim(); } + int getControlDim() const { return system_->getControlDim(); } + int getTotalDualDim() const { return total_dual_dim_; } + const CDDPOptions &getOptions() const { return options_; } + + // Setters + /** + * @brief Set the Dynamical System object + * @param system Dynamical system object (unique_ptr) + */ + void setDynamicalSystem(std::unique_ptr system) { system_ = std::move(system); } + + /** + * @brief Set the Initial state + * @param initial_state Initial state + */ + void setInitialState(const Eigen::VectorXd &initial_state) { initial_state_ = initial_state; } + + /** + * @brief Set the Reference state + * @param reference_state Reference state + */ + void setReferenceState(const Eigen::VectorXd &reference_state) + { + reference_state_ = reference_state; + // Update the objective reference state + objective_->setReferenceState(reference_state); + } + + void setReferenceStates(const std::vector &reference_states) + { + reference_states_ = reference_states; + // Update the objective reference states + objective_->setReferenceStates(reference_states); + } + + /** + * @brief Set the time horizon for the problem + * @param horizon Time horizon + */ + void setHorizon(int horizon) { horizon_ = horizon; } + + /** + * @brief Set the time step for the problem + * @param timestep Time step + */ + void setTimestep(double timestep) { timestep_ = timestep; } + + /** + * @brief Set the options for the solver + * @param options Solver options + */ + void setOptions(const CDDPOptions &options) { options_ = options; } + + /** + * @brief Set the Objective function + * @param objective Objective function object (unique_ptr) + */ + void setObjective(std::unique_ptr objective) { objective_ = std::move(objective); } + + /** + * @brief Set the Initial Trajectory + * @param X state trajectory + * @param U control trajectory + */ + void setInitialTrajectory(const std::vector &X, const std::vector &U); + + /** + * @brief Add a constraint to the problem + * + * @param constraint_name constraint name given by the user + * @param constraint constraint object + */ + void addConstraint(std::string constraint_name, std::unique_ptr constraint) + { + // Insert into the map + constraint_set_[constraint_name] = std::move(constraint); + + // Increment total_dual_dim_ + int dim = constraint_set_[constraint_name]->getDualDim(); + total_dual_dim_ += dim; + + initialized_ = false; // Reset the initialization flag + } + + /** + * @brief Get a specific constraint by name + * + * @tparam T Type of constraint + * @param name Name of the constraint + * @return T* Pointer to the constraint + */ + // Get a specific constraint by name + template + T *getConstraint(const std::string &name) const + { + auto it = constraint_set_.find(name); + + // For other constraints, return nullptr if not found + if (it == constraint_set_.end()) + { + return nullptr; + } + + // Try to cast to the requested type + T *cast_constraint = dynamic_cast(it->second.get()); + if (!cast_constraint) + { + return nullptr; + } + + return cast_constraint; } - // Try to cast to the requested type - T* cast_constraint = dynamic_cast(it->second.get()); - if (!cast_constraint) { - return nullptr; + // Getter for the constraint set + const std::map> &getConstraintSet() const + { + return constraint_set_; } - return cast_constraint; - } - - - // Getter for the constraint set - const std::map>& getConstraintSet() const { - return constraint_set_; - } - - // Initialization methods - void initializeCDDP(); - - // Solve the problem - CDDPSolution solve(std::string solver_type = "CLCDDP"); - -private: - // Solver methods - // CLCDDP methods - CDDPSolution solveCLCDDP(); - ForwardPassResult solveCLCDDPForwardPass(double alpha); - bool solveCLCDDPBackwardPass(); - - // LogCDDP methods - CDDPSolution solveLogCDDP(); - ForwardPassResult solveLogCDDPForwardPass(double alpha); - bool solveLogCDDPBackwardPass(); - - // ASCDDP methods - CDDPSolution solveASCDDP(); - ForwardPassResult solveASCDDPForwardPass(double alpha); - bool solveASCDDPBackwardPass(); - - // IPDDP methods - void initializeIPDDP(); - CDDPSolution solveIPDDP(); - ForwardPassResult solveIPDDPForwardPass(double alpha); - bool solveIPDDPBackwardPass(); - - // Feasible IPDDP methods - void initializeFeasibleIPDDP(); - CDDPSolution solveFeasibleIPDDP(); - ForwardPassResult solveFeasibleIPDDPForwardPass(double alpha); - bool solveFeasibleIPDDPBackwardPass(); - void resetIPDDPFilter(); - void initialIPDDPRollout(); - void resetIPDDPRegularization(); - - // 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); - - // Regularization methods - void increaseRegularization(); - void decreaseRegularization(); - bool isRegularizationLimitReached() const; - - // Print methods - void printSolverInfo(); - void printOptions(const CDDPOptions& options); - void printIteration(int iter, double cost, double lagrangian, - double grad_norm, double lambda_state, - double lambda_control, double alpha, double mu, double constraint_violation); - void printSolution(const CDDPSolution& solution); - -private: - bool initialized_ = false; // Initialization flag - - // Problem Data - std::unique_ptr system_; - std::unique_ptr objective_; - std::map> constraint_set_; - std::unique_ptr log_barrier_; - Eigen::VectorXd initial_state_; - Eigen::VectorXd reference_state_; // Desired reference state - std::vector reference_states_; // Desired reference states (trajectory) - int horizon_; // Time horizon for the problem - double timestep_; // Time step for the problem - CDDPOptions options_; // Options for the solver - - int total_dual_dim_ = 0; // Number of total dual variables across constraints - - // Intermediate trajectories - std::vector X_; // State trajectory - std::vector U_; // Control trajectory - std::map> G_; // Constraint trajectory - std::map> Y_; // Dual trajectory - std::map> S_; // Slack trajectory - - // Cost and Lagrangian - double J_; // Cost - double dJ_; // Cost improvement - double L_; // Lagrangian - double dL_; // Lagrangian improvement - Eigen::VectorXd dV_; - - // Line search - double alpha_; // Line search step size - std::vector alphas_; - - // Log-barrier - double mu_; // Barrier coefficient - std::vector filter_; // [logcost, error measure - int ipddp_regularization_counter_ = 0; // Regularization counter for IPDDP - double constraint_violation_; // Current constraint violation measure - double gamma_; // Small value for filter acceptance - - // Feedforward and feedback gains - std::vector k_u_; - std::vector K_u_; - std::map> k_y_; - std::map> K_y_; - std::map> k_s_; - std::map> K_s_; - - // Q-function matrices - std::vector Q_UU_; - std::vector Q_UX_; - std::vector Q_U_; - - // Regularization parameters - double regularization_state_; - double regularization_state_step_; - double regularization_control_; - double regularization_control_step_; - - // Boxqp solver - BoxQPOptions boxqp_options_; - BoxQPSolver boxqp_solver_; - - double optimality_gap_ = 1e+10; -}; + // Initialization methods + void initializeCDDP(); + + // Solve the problem + CDDPSolution solve(std::string solver_type = "CLCDDP"); + + private: + // Solver methods + // CLCDDP methods + CDDPSolution solveCLCDDP(); + ForwardPassResult solveCLCDDPForwardPass(double alpha); + bool solveCLCDDPBackwardPass(); + + // LogCDDP methods + CDDPSolution solveLogCDDP(); + ForwardPassResult solveLogCDDPForwardPass(double alpha); + bool solveLogCDDPBackwardPass(); + + // ASCDDP methods + CDDPSolution solveASCDDP(); + ForwardPassResult solveASCDDPForwardPass(double alpha); + bool solveASCDDPBackwardPass(); + + // IPDDP methods + void initializeIPDDP(); + CDDPSolution solveIPDDP(); + ForwardPassResult solveIPDDPForwardPass(double alpha); + bool solveIPDDPBackwardPass(); + + // Feasible IPDDP methods + void initializeFeasibleIPDDP(); + CDDPSolution solveFeasibleIPDDP(); + ForwardPassResult solveFeasibleIPDDPForwardPass(double alpha); + bool solveFeasibleIPDDPBackwardPass(); + void resetIPDDPFilter(); + void initialIPDDPRollout(); + void resetIPDDPRegularization(); + + // 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); + + // Regularization methods + void increaseRegularization(); + void decreaseRegularization(); + bool isRegularizationLimitReached() const; + + // Print methods + void printSolverInfo(); + void printOptions(const CDDPOptions &options); + void printIteration(int iter, double cost, double lagrangian, + double grad_norm, double lambda_state, + double lambda_control, double alpha, double mu, double constraint_violation); + void printSolution(const CDDPSolution &solution); + + private: + bool initialized_ = false; // Initialization flag + + // Problem Data + std::unique_ptr system_; + std::unique_ptr objective_; + std::map> constraint_set_; + std::unique_ptr log_barrier_; + Eigen::VectorXd initial_state_; + Eigen::VectorXd reference_state_; // Desired reference state + std::vector reference_states_; // Desired reference states (trajectory) + int horizon_; // Time horizon for the problem + double timestep_; // Time step for the problem + CDDPOptions options_; // Options for the solver + + int total_dual_dim_ = 0; // Number of total dual variables across constraints + + // Intermediate trajectories + std::vector X_; // State trajectory + std::vector U_; // Control trajectory + std::map> G_; // Constraint trajectory + std::map> Y_; // Dual trajectory + std::map> S_; // Slack trajectory + + // Cost and Lagrangian + double J_; // Cost + double dJ_; // Cost improvement + double L_; // Lagrangian + double dL_; // Lagrangian improvement + Eigen::VectorXd dV_; + + // Line search + double alpha_; // Line search step size + std::vector alphas_; + + // Line search step sizes for IPDDP + double alpha_control_; // Line search step size for control + double alpha_slack_; // Line search step size for slack + double alpha_dual_; // Line search step size for dual + + // Log-barrier + double mu_; // Barrier coefficient + std::vector filter_; // [logcost, error measure + int ipddp_regularization_counter_ = 0; // Regularization counter for IPDDP + double constraint_violation_; // Current constraint violation measure + double gamma_; // Small value for filter acceptance + + // Feedforward and feedback gains + std::vector k_u_; + std::vector K_u_; + std::map> k_y_; + std::map> K_y_; + std::map> k_s_; + std::map> K_s_; + + // Q-function matrices + std::vector Q_UU_; + std::vector Q_UX_; + std::vector Q_U_; + + // Regularization parameters + double regularization_state_; + double regularization_state_step_; + double regularization_control_; + double regularization_control_step_; + + // Boxqp solver + BoxQPOptions boxqp_options_; + BoxQPSolver boxqp_solver_; + + double optimality_gap_ = 1e+10; + }; } // namespace cddp #endif // CDDP_CDDP_CORE_HPP \ No newline at end of file