From 2341f9da916c87bc5ccbb727db16c395f15bb4b5 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 02:55:12 -0400 Subject: [PATCH 1/7] Refactor and simplify core library - Unify ControlConstraint/StateConstraint into BoxConstraint<> template - Add default zero-Hessian implementations in Constraint base class - Extract InteriorPointOptions/MultiShootingOptions base structs in options - Create CDDPSolverBase with template method solve() pattern - Migrate all 4 solvers (CLDDP, LogDDP, IPDDP, MSIPDDP) to use base class - Add DynamicalSystem::makeZeroTensor() helper, update 16 dynamics models - Consolidate 9 finite difference functions into 3 unified templates --- CMakeLists.txt | 1 + .../cddp-cpp/cddp_core/cddp_solver_base.hpp | 179 ++++ include/cddp-cpp/cddp_core/clddp_solver.hpp | 83 +- include/cddp-cpp/cddp_core/constraint.hpp | 395 +------- .../cddp-cpp/cddp_core/dynamical_system.hpp | 5 + include/cddp-cpp/cddp_core/helper.hpp | 422 +++------ include/cddp-cpp/cddp_core/ipddp_solver.hpp | 206 +--- include/cddp-cpp/cddp_core/logddp_solver.hpp | 140 +-- include/cddp-cpp/cddp_core/msipddp_solver.hpp | 284 ++---- include/cddp-cpp/cddp_core/options.hpp | 97 +- src/cddp_core/cddp_solver_base.cpp | 411 ++++++++ src/cddp_core/clddp_solver.cpp | 401 +------- src/cddp_core/ipddp_solver.cpp | 744 +++------------ src/cddp_core/logddp_solver.cpp | 484 ++-------- src/cddp_core/msipddp_solver.cpp | 891 ++++-------------- src/dynamics_model/bicycle.cpp | 10 +- src/dynamics_model/car.cpp | 10 +- src/dynamics_model/dreyfus_rocket.cpp | 10 +- src/dynamics_model/dubins_car.cpp | 10 +- src/dynamics_model/forklift.cpp | 10 +- src/dynamics_model/lti_system.cpp | 10 +- src/dynamics_model/manipulator.cpp | 10 +- src/dynamics_model/pendulum.cpp | 10 +- src/dynamics_model/spacecraft_landing2d.cpp | 10 +- src/dynamics_model/spacecraft_linear.cpp | 10 +- src/dynamics_model/spacecraft_linear_fuel.cpp | 15 +- src/dynamics_model/spacecraft_nonlinear.cpp | 10 +- src/dynamics_model/spacecraft_roe.cpp | 12 +- src/dynamics_model/spacecraft_twobody.cpp | 10 +- src/dynamics_model/unicycle.cpp | 10 +- src/dynamics_model/usv_3dof.cpp | 5 +- 31 files changed, 1437 insertions(+), 3468 deletions(-) create mode 100644 include/cddp-cpp/cddp_core/cddp_solver_base.hpp create mode 100644 src/cddp_core/cddp_solver_base.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cb1ac82..3b258bb3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,6 +139,7 @@ set(cddp_core_srcs src/cddp_core/boxqp.cpp src/cddp_core/qp_solver.cpp src/cddp_core/cddp_core.cpp + src/cddp_core/cddp_solver_base.cpp src/cddp_core/clddp_solver.cpp src/cddp_core/logddp_solver.cpp src/cddp_core/ipddp_solver.cpp diff --git a/include/cddp-cpp/cddp_core/cddp_solver_base.hpp b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp new file mode 100644 index 00000000..722ce00d --- /dev/null +++ b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp @@ -0,0 +1,179 @@ +/* + Copyright 2025 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_SOLVER_BASE_HPP +#define CDDP_SOLVER_BASE_HPP + +#include "cddp_core/cddp_core.hpp" +#include +#include +#include + +namespace cddp { + +/** + * @brief Base class for all DDP solver implementations. + * + * Provides a template method solve() that defines the common iteration + * structure shared by CLDDP, LogDDP, IPDDP, and MSIPDDP. Subclasses + * override virtual hooks to customize solver-specific behavior. + */ +class CDDPSolverBase : public ISolverAlgorithm { +public: + virtual ~CDDPSolverBase() = default; + + /** + * @brief Main solve loop (template method). Marked final. + * + * Subclasses customize behavior via protected virtual hooks. + */ + CDDPSolution solve(CDDP &context) override final; + +protected: + // === Common member variables === + std::vector k_u_; ///< Feedforward control gains + std::vector K_u_; ///< Feedback control gains + Eigen::Vector2d dV_; ///< Expected value function change + + // Dynamics derivatives (pre-computed by barrier/IP solvers) + std::vector F_x_; ///< State Jacobians + std::vector F_u_; ///< Control Jacobians + std::vector> F_xx_, F_uu_, + F_ux_; ///< Dynamics Hessians + + // === Virtual hooks for solver customization === + + /** + * @brief Called before the main loop. Evaluate trajectory, reset filter, etc. + */ + virtual void preIterationSetup(CDDP &context) {} + + /** + * @brief Perform the backward pass (Riccati recursion). + * @return True if successful, false if regularization needed. + */ + virtual bool backwardPass(CDDP &context) = 0; + + /** + * @brief Perform a single forward pass trial with given step size. + */ + virtual ForwardPassResult forwardPass(CDDP &context, double alpha) = 0; + + /** + * @brief Apply a successful forward pass result to update context state. + * Default: updates X_, U_, cost_, merit_function_, alpha_pr_. + * Override to also update dual/slack/costate variables. + */ + virtual void applyForwardPassResult(CDDP &context, + const ForwardPassResult &result); + + /** + * @brief Check convergence criteria. + * @param dJ Change in cost from last iteration. + * @param dL Change in merit/Lagrangian from last iteration. + * @param iter Current iteration number. + * @param[out] reason Termination reason string if converged. + * @return True if converged. + */ + virtual bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) = 0; + + /** + * @brief Called after each iteration (barrier parameter update, etc.). + * @param forward_pass_success Whether the forward pass succeeded. + */ + virtual void postIterationUpdate(CDDP &context, bool forward_pass_success) {} + + /** + * @brief Handle forward pass failure. Default: increase regularization. + * Override for filter restoration (MSIPDDP). + * @return True if the solver should break out of the main loop. + */ + virtual bool handleForwardPassFailure(CDDP &context, + std::string &termination_reason); + + /** + * @brief Record iteration history. Called after successful forward pass. + * Default records objective, merit, step lengths, regularization. + * Override to add barrier/IP specific metrics. + */ + virtual void recordIterationHistory(const CDDP &context); + + /** + * @brief Populate solver-specific fields in the solution. + * Called after the main loop. Override to add barrier/dual/slack fields. + */ + virtual void populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) {} + + /** + * @brief Print iteration info. Solver-specific column layouts. + */ + virtual void printIteration(int iter, const CDDP &context) const = 0; + + /** + * @brief Print solution summary. + */ + virtual void printSolutionSummary(const CDDPSolution &solution) const; + + // === Shared implementations === + + /** + * @brief Perform forward pass with line search (sequential or parallel). + */ + ForwardPassResult performForwardPass(CDDP &context); + + /** + * @brief Pre-compute dynamics Jacobians and optionally Hessians. + */ + void precomputeDynamicsDerivatives(CDDP &context, + int min_horizon_for_parallel = 50); + + /** + * @brief Initialize control gain storage to zeros. + */ + void initializeGains(int horizon, int control_dim, int state_dim); + + /** + * @brief Build time points vector from context. + */ + static std::vector buildTimePoints(const CDDP &context); + + /** + * @brief Compute trajectory cost (running + terminal). + */ + void computeCost(CDDP &context); + + // === History tracking === + struct IterationHistory { + std::vector objective; + std::vector merit_function; + std::vector step_length_primal; + std::vector step_length_dual; + std::vector dual_infeasibility; + std::vector primal_infeasibility; + std::vector complementary_infeasibility; + std::vector barrier_mu; + std::vector regularization; + + void reserve(size_t n); + void clear(); + } history_; +}; + +} // namespace cddp + +#endif // CDDP_SOLVER_BASE_HPP diff --git a/include/cddp-cpp/cddp_core/clddp_solver.hpp b/include/cddp-cpp/cddp_core/clddp_solver.hpp index 0a395002..2b25f552 100644 --- a/include/cddp-cpp/cddp_core/clddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/clddp_solver.hpp @@ -18,7 +18,7 @@ #define CDDP_CLDDP_SOLVER_HPP #include "cddp_core/boxqp.hpp" -#include "cddp_core/cddp_core.hpp" +#include "cddp_core/cddp_solver_base.hpp" #include "cddp_core/constraint.hpp" #include #include @@ -27,86 +27,23 @@ namespace cddp { /** * @brief Constrained Linear DDP (CLDDP) solver implementation. - * - * This class implements the ISolverAlgorithm interface to provide - * a constrained linear DDP solver with box constraints on controls. */ -class CLDDPSolver : public ISolverAlgorithm { +class CLDDPSolver : public CDDPSolverBase { public: - /** - * @brief Default constructor. - */ CLDDPSolver(); - /** - * @brief Initialize the solver with the given CDDP context. - * @param context Reference to the CDDP instance containing problem data and - * options. - */ void initialize(CDDP &context) override; - - /** - * @brief Execute the CLDDP algorithm and return the solution. - * @param context Reference to the CDDP instance containing problem data and - * options. - * @return CDDPSolution containing the results. - */ - CDDPSolution solve(CDDP &context) override; - - /** - * @brief Get the name of the solver algorithm. - * @return String identifier "CLDDP". - */ std::string getSolverName() const override; -private: - // Control law parameters - std::vector k_u_; ///< Feedforward control gains - std::vector K_u_; ///< Feedback control gains - Eigen::Vector2d dV_; ///< Expected value function change - - // Constraint solver - BoxQPSolver boxqp_solver_; ///< Box QP solver for control constraints - - /** - * @brief Perform backward pass (Riccati recursion). - * @param context Reference to the CDDP context. - * @return True if backward pass succeeds, false otherwise. - */ - bool backwardPass(CDDP &context); +protected: + bool backwardPass(CDDP &context) override; + ForwardPassResult forwardPass(CDDP &context, double alpha) override; + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override; + void printIteration(int iter, const CDDP &context) const override; - /** - * @brief Perform forward pass with line search. - * @param context Reference to the CDDP context. - * @return Best forward pass result. - */ - ForwardPassResult performForwardPass(CDDP &context); - - /** - * @brief Perform single forward pass with given step size. - * @param context Reference to the CDDP context. - * @param alpha Step size for the forward pass. - * @return Forward pass result. - */ - ForwardPassResult forwardPass(CDDP &context, double alpha); - - /** - * @brief Compute the current cost given the trajectories. - * @param context Reference to the CDDP context. - */ - void computeCost(CDDP &context); - - /** - * @brief Print iteration information. - */ - void printIteration(int iter, double cost, double merit, double inf_du, - double regularization, double alpha) const; - - /** - * @brief Print solution summary. - * @param solution The solution to print. - */ - void printSolutionSummary(const CDDPSolution &solution) const; +private: + BoxQPSolver boxqp_solver_; }; } // namespace cddp diff --git a/include/cddp-cpp/cddp_core/constraint.hpp b/include/cddp-cpp/cddp_core/constraint.hpp index 83707a37..ec4e8319 100644 --- a/include/cddp-cpp/cddp_core/constraint.hpp +++ b/include/cddp-cpp/cddp_core/constraint.hpp @@ -88,33 +88,36 @@ namespace cddp // Hessian of the constraint w.r.t the state: d^2g/dx^2 // Returns a vector of matrices, one for each output dimension of g(x,u) + // Default: zero Hessians (correct for linear constraints) virtual std::vector getStateHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index = 0) const { - throw std::logic_error( - "getStateHessian not implemented for this constraint type."); + return std::vector( + getDualDim(), Eigen::MatrixXd::Zero(state.size(), state.size())); } // Hessian of the constraint w.r.t the control: d^2g/du^2 + // Default: zero Hessians (correct for linear constraints) virtual std::vector getControlHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index = 0) const { - throw std::logic_error( - "getControlHessian not implemented for this constraint type."); + return std::vector( + getDualDim(), Eigen::MatrixXd::Zero(control.size(), control.size())); } // Mixed Hessian of the constraint w.r.t state and control: d^2g/dudx + // Default: zero Hessians (correct for linear constraints) virtual std::vector getCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index = 0) const { - throw std::logic_error( - "getCrossHessian not implemented for this constraint type."); + return std::vector( + getDualDim(), Eigen::MatrixXd::Zero(control.size(), state.size())); } // Utility: Get all Hessians: d^2g/dx^2, d^2g/du^2, d^2g/dudx @@ -134,14 +137,18 @@ namespace cddp //------------------------------------------------------------------------------ - class ControlConstraint : public Constraint + enum class BoxVariable { State, Control }; + + template + class BoxConstraint : public Constraint { public: - ControlConstraint(const Eigen::VectorXd &lower_bound, - const Eigen::VectorXd &upper_bound, - double scale_factor = 1.0) - : Constraint("ControlConstraint"), lower_bound_(lower_bound), - upper_bound_(upper_bound), scale_factor_(scale_factor) + BoxConstraint(const Eigen::VectorXd &lower_bound, + const Eigen::VectorXd &upper_bound, + double scale_factor = 1.0) + : Constraint(Var == BoxVariable::Control ? "ControlConstraint" : "StateConstraint"), + lower_bound_(lower_bound), upper_bound_(upper_bound), + scale_factor_(scale_factor) { dim_ = 2 * upper_bound.size(); ip_upper_bound_.resize(dim_); @@ -151,13 +158,14 @@ namespace cddp int getDualDim() const override { return dim_; } - Eigen::VectorXd evaluate(const Eigen::VectorXd & /**/, + Eigen::VectorXd evaluate(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index = 0) const override { + const auto &var = (Var == BoxVariable::Control) ? control : state; Eigen::VectorXd g(dim_); - g.head(control.size()) = -control; - g.tail(control.size()) = control; + g.head(var.size()) = -var; + g.tail(var.size()) = var; return g * scale_factor_; } @@ -174,136 +182,19 @@ namespace cddp const Eigen::VectorXd &control, int index = 0) const override { - return Eigen::MatrixXd::Zero(dim_, state.size()); - } - - Eigen::MatrixXd - getControlJacobian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - Eigen::MatrixXd jac(dim_, control.size()); - jac.topLeftCorner(control.size(), control.size()) = - -Eigen::MatrixXd::Identity(control.size(), control.size()); - jac.bottomRightCorner(control.size(), control.size()) = - Eigen::MatrixXd::Identity(control.size(), control.size()); - return jac; - } - - // Raw bounds for CLDDP / BoxQP solvers - const Eigen::VectorXd &rawLowerBound() const { return lower_bound_; } - const Eigen::VectorXd &rawUpperBound() const { return upper_bound_; } - - Eigen::VectorXd clamp(const Eigen::VectorXd &control) const - { - return control.cwiseMax(lower_bound_).cwiseMin(upper_bound_); - } - - double computeViolation(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - Eigen::VectorXd g = evaluate(state, control, index) - ip_upper_bound_; - return computeViolationFromValue(g); - } - - double computeViolationFromValue(const Eigen::VectorXd &g) const override - { - return (g - ip_upper_bound_).cwiseMax(0.0).sum(); - } - - // Hessians for ControlConstraint are zero - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hxx_list; - for (int i = 0; i < dim_; ++i) - { - Hxx_list.push_back(Eigen::MatrixXd::Zero(state.size(), state.size())); - } - return Hxx_list; - } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Huu_list; - for (int i = 0; i < dim_; ++i) + if constexpr (Var == BoxVariable::Control) { - Huu_list.push_back(Eigen::MatrixXd::Zero(control.size(), control.size())); + return Eigen::MatrixXd::Zero(dim_, state.size()); } - return Huu_list; - } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hux_list; - for (int i = 0; i < dim_; ++i) + else { - Hux_list.push_back(Eigen::MatrixXd::Zero(control.size(), state.size())); + Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(dim_, state.size()); + jac.topLeftCorner(state.size(), state.size()) = + -Eigen::MatrixXd::Identity(state.size(), state.size()) * scale_factor_; + jac.bottomRightCorner(state.size(), state.size()) = + Eigen::MatrixXd::Identity(state.size(), state.size()) * scale_factor_; + return jac; } - return Hux_list; - } - - private: - Eigen::VectorXd lower_bound_; - Eigen::VectorXd upper_bound_; - Eigen::VectorXd ip_upper_bound_; - double scale_factor_; - int dim_; - }; - - class StateConstraint : public Constraint - { - public: - StateConstraint(const Eigen::VectorXd &lower_bound, - const Eigen::VectorXd &upper_bound, - double scale_factor = 1.0) - : Constraint("StateConstraint"), lower_bound_(lower_bound), - upper_bound_(upper_bound), scale_factor_(scale_factor) - { - dim_ = 2 * upper_bound.size(); - ip_upper_bound_.resize(dim_); - ip_upper_bound_.head(upper_bound.size()) = -lower_bound * scale_factor_; - ip_upper_bound_.tail(upper_bound.size()) = upper_bound * scale_factor_; - } - - int getDualDim() const override { return dim_; } - - Eigen::VectorXd evaluate(const Eigen::VectorXd &state, - const Eigen::VectorXd & /**/, - int index = 0) const override - { - Eigen::VectorXd g(dim_); - g.head(state.size()) = -state; - g.tail(state.size()) = state; - return g * scale_factor_; - } - - Eigen::VectorXd getLowerBound() const override - { - return Eigen::VectorXd::Constant(dim_, - -std::numeric_limits::infinity()); - } - - Eigen::VectorXd getUpperBound() const override { return ip_upper_bound_; } - - Eigen::MatrixXd - getStateJacobian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - Eigen::MatrixXd jac(dim_, state.size()); - jac.topLeftCorner(state.size(), state.size()) = - -Eigen::MatrixXd::Identity(state.size(), state.size()) * scale_factor_; - jac.bottomRightCorner(state.size(), state.size()) = - Eigen::MatrixXd::Identity(state.size(), state.size()) * scale_factor_; - return jac; } Eigen::MatrixXd @@ -311,24 +202,34 @@ namespace cddp const Eigen::VectorXd &control, int index = 0) const override { - return Eigen::MatrixXd::Zero(dim_, control.size()); + if constexpr (Var == BoxVariable::State) + { + return Eigen::MatrixXd::Zero(dim_, control.size()); + } + else + { + Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(dim_, control.size()); + jac.topLeftCorner(control.size(), control.size()) = + -Eigen::MatrixXd::Identity(control.size(), control.size()); + jac.bottomRightCorner(control.size(), control.size()) = + Eigen::MatrixXd::Identity(control.size(), control.size()); + return jac; + } } - // Raw bounds for potential CLDDP / BoxQP usage const Eigen::VectorXd &rawLowerBound() const { return lower_bound_; } const Eigen::VectorXd &rawUpperBound() const { return upper_bound_; } - Eigen::VectorXd clamp(const Eigen::VectorXd &state) const + Eigen::VectorXd clamp(const Eigen::VectorXd &v) const { - return state.cwiseMax(lower_bound_).cwiseMin(upper_bound_); + return v.cwiseMax(lower_bound_).cwiseMin(upper_bound_); } double computeViolation(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index = 0) const override { - Eigen::VectorXd g = evaluate(state, control, index) - ip_upper_bound_; - return computeViolationFromValue(g); + return computeViolationFromValue(evaluate(state, control, index) - ip_upper_bound_); } double computeViolationFromValue(const Eigen::VectorXd &g) const override @@ -336,44 +237,6 @@ namespace cddp return (g - ip_upper_bound_).cwiseMax(0.0).sum(); } - // Hessians for StateConstraint are zero - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hxx_list; - for (int i = 0; i < dim_; ++i) - { - Hxx_list.push_back(Eigen::MatrixXd::Zero(state.size(), state.size())); - } - return Hxx_list; - } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Huu_list; - for (int i = 0; i < dim_; ++i) - { - Huu_list.push_back(Eigen::MatrixXd::Zero(control.size(), control.size())); - } - return Huu_list; - } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hux_list; - for (int i = 0; i < dim_; ++i) - { - Hux_list.push_back(Eigen::MatrixXd::Zero(control.size(), state.size())); - } - return Hux_list; - } - private: Eigen::VectorXd lower_bound_; Eigen::VectorXd upper_bound_; @@ -382,6 +245,9 @@ namespace cddp int dim_; }; + using ControlConstraint = BoxConstraint; + using StateConstraint = BoxConstraint; + class LinearConstraint : public Constraint { public: @@ -436,44 +302,6 @@ namespace cddp return std::max(0.0, (b_ - g).maxCoeff()); } - // Hessians for LinearConstraint are zero - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hxx_list; - for (int i = 0; i < A_.rows(); ++i) - { - Hxx_list.push_back(Eigen::MatrixXd::Zero(state.size(), state.size())); - } - return Hxx_list; - } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Huu_list; - for (int i = 0; i < A_.rows(); ++i) - { - Huu_list.push_back(Eigen::MatrixXd::Zero(control.size(), control.size())); - } - return Huu_list; - } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hux_list; - for (int i = 0; i < A_.rows(); ++i) - { - Hux_list.push_back(Eigen::MatrixXd::Zero(control.size(), state.size())); - } - return Hux_list; - } - private: Eigen::MatrixXd A_; Eigen::VectorXd b_; @@ -566,22 +394,6 @@ namespace cddp return {Hxx}; } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - return {Eigen::MatrixXd::Zero(control.size(), control.size())}; - } - - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - return {Eigen::MatrixXd::Zero(control.size(), state.size())}; - } - private: double radius_; Eigen::VectorXd center_; @@ -782,37 +594,6 @@ namespace cddp return std::max(0.0, g(0)); } - // Hessians for PoleConstraint - // TODO: Implement actual Hessians for PoleConstraint - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getStateHessian for PoleConstraint not yet implemented."); - Eigen::MatrixXd Hxx = Eigen::MatrixXd::Zero(state.size(), state.size()); - return {Hxx}; - } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getControlHessian for PoleConstraint not yet implemented."); - return {Eigen::MatrixXd::Zero(control.size(), control.size())}; - } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getCrossHessian for PoleConstraint not yet implemented."); - return {Eigen::MatrixXd::Zero(control.size(), state.size())}; - } - private: Eigen::Vector3d center_; // Center of the cylinder. Eigen::Vector3d axis_; // Unit vector for the cylinder's axis. @@ -980,37 +761,6 @@ namespace cddp return std::max(0.0, g(0)); } - // Hessians for SecondOrderConeConstraint - // TODO: Implement actual Hessians for SecondOrderConeConstraint - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getStateHessian for SecondOrderConeConstraint not yet implemented."); - Eigen::MatrixXd Hxx = Eigen::MatrixXd::Zero(state.size(), state.size()); - return {Hxx}; - } - std::vector - getControlHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getControlHessian for SecondOrderConeConstraint not yet implemented."); - return {Eigen::MatrixXd::Zero(control.size(), control.size())}; - } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - throw std::logic_error( - "getCrossHessian for SecondOrderConeConstraint not yet implemented."); - return {Eigen::MatrixXd::Zero(control.size(), state.size())}; - } - private: Eigen::Vector3d p_o_; // Cone origin position Eigen::Vector3d @@ -1118,17 +868,6 @@ namespace cddp return std::max(0.0, g(0)) + std::max(0.0, g(1)); } - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd & /*control*/, - int index = 0) const override - { - std::vector Hxx_list; - Hxx_list.push_back(Eigen::MatrixXd::Zero(state.size(), state.size())); - Hxx_list.push_back(Eigen::MatrixXd::Zero(state.size(), state.size())); - return Hxx_list; - } - std::vector getControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, @@ -1141,10 +880,7 @@ namespace cddp double denominator = std::pow(term_in_sqrt, 1.5); - if (denominator > - std::numeric_limits:: - min()) // Avoid division by zero if denominator underflows, though - // epsilon should prevent this for reasonable values + if (denominator > std::numeric_limits::min()) { H_norm_reg = (term_in_sqrt * Eigen::MatrixXd::Identity(control.size(), control.size()) - @@ -1155,17 +891,6 @@ namespace cddp return {-H_norm_reg, H_norm_reg}; } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - std::vector Hux_list; - Hux_list.push_back(Eigen::MatrixXd::Zero(control.size(), state.size())); - Hux_list.push_back(Eigen::MatrixXd::Zero(control.size(), state.size())); - return Hux_list; - } - private: double min_thrust_norm_; double max_thrust_norm_; @@ -1260,14 +985,6 @@ namespace cddp return std::max(0.0, g(0)); // Violation if g(0) > 0 } - std::vector - getStateHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd & /*control*/, - int index = 0) const override - { - return {Eigen::MatrixXd::Zero(state.size(), state.size())}; - } - std::vector getControlHessian(const Eigen::VectorXd & /*state*/, const Eigen::VectorXd &control, @@ -1289,14 +1006,6 @@ namespace cddp return {H_norm_reg}; } - std::vector - getCrossHessian(const Eigen::VectorXd &state, - const Eigen::VectorXd &control, - int index = 0) const override - { - return {Eigen::MatrixXd::Zero(control.size(), state.size())}; - } - private: double max_thrust_norm_; double epsilon_; diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 0c67124b..0d886c77 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -125,6 +125,11 @@ class DynamicalSystem { double getTimestep() const { return timestep_; } std::string getIntegrationType() const { return integration_type_; } + // Utility: create a vector of zero matrices (for Hessian tensors) + static std::vector makeZeroTensor(int count, int rows, int cols) { + return std::vector(count, Eigen::MatrixXd::Zero(rows, cols)); + } + protected: int state_dim_; int control_dim_; diff --git a/include/cddp-cpp/cddp_core/helper.hpp b/include/cddp-cpp/cddp_core/helper.hpp index d88cf987..432c2d46 100644 --- a/include/cddp-cpp/cddp_core/helper.hpp +++ b/include/cddp-cpp/cddp_core/helper.hpp @@ -23,104 +23,11 @@ namespace cddp { /** - * @brief Compute gradient using central finite differences - * @param f Function to differentiate + * @brief Compute gradient using finite differences + * @param f Scalar function to differentiate * @param x Point at which to evaluate gradient * @param h Step size for finite differences - * @return Gradient vector - */ - template - Eigen::VectorXd finite_difference_gradient_central(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::VectorXd grad(n); - - // Compute central differences - Eigen::VectorXd x_plus = x; - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - x_minus(i) = x(i) - h; - - grad(i) = (f(x_plus) - f(x_minus)) / (2.0 * h); - - x_plus(i) = x(i); - x_minus(i) = x(i); - } - - return grad; - } - - /** - * @brief Compute gradient using forward finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate gradient - * @param h Step size for finite differences - * @return Gradient vector - */ - template - Eigen::VectorXd finite_difference_gradient_forward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::VectorXd grad(n); - - // Compute forward differences - Eigen::VectorXd x_plus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - - grad(i) = (f(x_plus) - f(x)) / h; - - x_plus(i) = x(i); - } - - return grad; - } - - /** - * @brief Compute gradient using backward finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate gradient - * @param h Step size for finite differences - * @return Gradient vector - */ - template - Eigen::VectorXd finite_difference_gradient_backward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::VectorXd grad(n); - - // Compute backward differences - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_minus(i) = x(i) - h; - - grad(i) = (f(x) - f(x_minus)) / h; - - x_minus(i) = x(i); - } - - return grad; - } - - /** - * @brief Compute gradient using central finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate gradient - * @param h Step size for finite differences (optional) - * @param mode mode for differentiating options: 0 for central, 1 for forward, 2 for backward + * @param mode 0 = central, 1 = forward, 2 = backward * @return Gradient vector */ template @@ -129,133 +36,60 @@ namespace cddp double h = 2e-5, int mode = 0) { + const int n = x.size(); + Eigen::VectorXd grad(n); + Eigen::VectorXd x_perturbed = x; + if (mode == 0) { - return finite_difference_gradient_central(f, x, h); + // Central differences + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + double f_plus = f(x_perturbed); + x_perturbed(i) = x(i) - h; + double f_minus = f(x_perturbed); + grad(i) = (f_plus - f_minus) / (2.0 * h); + x_perturbed(i) = x(i); + } } else if (mode == 1) { - return finite_difference_gradient_forward(f, x, h); + // Forward differences + const double f_x = f(x); + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + grad(i) = (f(x_perturbed) - f_x) / h; + x_perturbed(i) = x(i); + } } else if (mode == 2) { - return finite_difference_gradient_backward(f, x, h); + // Backward differences + const double f_x = f(x); + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) - h; + grad(i) = (f_x - f(x_perturbed)) / h; + x_perturbed(i) = x(i); + } } else { std::cerr << "Invalid mode value for finite difference gradient" << std::endl; - return Eigen::VectorXd::Zero(x.size()); - } - return Eigen::VectorXd::Zero(x.size()); - } - - /** - * @brief Compute Jacobian using central finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Jacobian - * @param h Step size for finite differences - * @return Jacobian matrix - */ - template - Eigen::MatrixXd finite_difference_jacobian_central(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int m = f(x).size(); - const int n = x.size(); - Eigen::MatrixXd jac(m, n); - - // Compute central differences - Eigen::VectorXd x_plus = x; - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - x_minus(i) = x(i) - h; - - Eigen::VectorXd f_plus = f(x_plus); - Eigen::VectorXd f_minus = f(x_minus); - - jac.col(i) = (f_plus - f_minus) / (2.0 * h); - - x_plus(i) = x(i); - x_minus(i) = x(i); - } - - return jac; - } - - /** - * @brief Compute Jacobian using forward finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Jacobian - * @param h Step size for finite differences - * @return Jacobian matrix - */ - template - Eigen::MatrixXd finite_difference_jacobian_forward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int m = f(x).size(); - const int n = x.size(); - Eigen::MatrixXd jac(m, n); - - // Compute forward differences - Eigen::VectorXd x_plus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - - Eigen::VectorXd f_plus = f(x_plus); - jac.col(i) = (f_plus - f(x)) / h; - - x_plus(i) = x(i); + return Eigen::VectorXd::Zero(n); } - return jac; + return grad; } /** - * @brief Compute Jacobian using backward finite differences - * @param f Function to differentiate + * @brief Compute Jacobian using finite differences + * @param f Vector function to differentiate * @param x Point at which to evaluate Jacobian * @param h Step size for finite differences - * @return Jacobian matrix - */ - template - Eigen::MatrixXd finite_difference_jacobian_backward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int m = f(x).size(); - const int n = x.size(); - Eigen::MatrixXd jac(m, n); - - // Compute backward differences - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_minus(i) = x(i) - h; - - Eigen::VectorXd f_minus = f(x_minus); - jac.col(i) = (f(x) - f_minus) / h; - - x_minus(i) = x(i); - } - - return jac; - } - - /* - * @brief Compute Jacobian using central finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Jacobian - * @param h Step size for finite differences (optional) - * @param mode mode for differentiating options: 0 for central, 1 for forward, 2 for backward + * @param mode 0 = central, 1 = forward, 2 = backward * @return Jacobian matrix */ template @@ -264,130 +98,60 @@ namespace cddp double h = 2e-5, int mode = 0) { + const Eigen::VectorXd f_x = f(x); + const int m = f_x.size(); + const int n = x.size(); + Eigen::MatrixXd jac(m, n); + Eigen::VectorXd x_perturbed = x; + if (mode == 0) { - return finite_difference_jacobian_central(f, x, h); + // Central differences + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + Eigen::VectorXd f_plus = f(x_perturbed); + x_perturbed(i) = x(i) - h; + Eigen::VectorXd f_minus = f(x_perturbed); + jac.col(i) = (f_plus - f_minus) / (2.0 * h); + x_perturbed(i) = x(i); + } } else if (mode == 1) { - return finite_difference_jacobian_forward(f, x, h); + // Forward differences + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + jac.col(i) = (f(x_perturbed) - f_x) / h; + x_perturbed(i) = x(i); + } } else if (mode == 2) { - return finite_difference_jacobian_backward(f, x, h); + // Backward differences + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) - h; + jac.col(i) = (f_x - f(x_perturbed)) / h; + x_perturbed(i) = x(i); + } } else { std::cerr << "Invalid mode value for finite difference Jacobian" << std::endl; - return Eigen::MatrixXd::Zero(f(x).size(), x.size()); - } - return Eigen::MatrixXd::Zero(f(x).size(), x.size()); - } - - /** - * @brief Compute Hessian using central finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Hessian - * @param h Step size for finite differences - * @return Hessian matrix - */ - template - Eigen::MatrixXd finite_difference_hessian_central(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::MatrixXd hess(n, n); - - // Compute central differences - Eigen::VectorXd x_plus = x; - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - x_minus(i) = x(i) - h; - - Eigen::VectorXd grad_plus = finite_difference_gradient(f, x_plus, h); - Eigen::VectorXd grad_minus = finite_difference_gradient(f, x_minus, h); - - hess.col(i) = (grad_plus - grad_minus) / (2.0 * h); - - x_plus(i) = x(i); - x_minus(i) = x(i); + return Eigen::MatrixXd::Zero(m, n); } - return hess; - } - - /** - * @brief Compute Hessian using forward finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Hessian - * @param h Step size for finite differences - * @return Hessian matrix - */ - template - Eigen::MatrixXd finite_difference_hessian_forward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::MatrixXd hess(n, n); - - // Compute forward differences - Eigen::VectorXd x_plus = x; - - for (int i = 0; i < n; ++i) - { - x_plus(i) = x(i) + h; - - Eigen::VectorXd grad_plus = finite_difference_gradient(f, x_plus, h); - hess.col(i) = (grad_plus - finite_difference_gradient(f, x, h)) / h; - - x_plus(i) = x(i); - } - - return hess; + return jac; } /** - * @brief Compute Hessian using backward finite differences - * @param f Function to differentiate + * @brief Compute Hessian using finite differences + * @param f Scalar function to differentiate * @param x Point at which to evaluate Hessian * @param h Step size for finite differences - * @return Hessian matrix - */ - template - Eigen::MatrixXd finite_difference_hessian_backward(const F &f, - const Eigen::VectorXd &x, - double h) - { - const int n = x.size(); - Eigen::MatrixXd hess(n, n); - - // Compute backward differences - Eigen::VectorXd x_minus = x; - - for (int i = 0; i < n; ++i) - { - x_minus(i) = x(i) - h; - - Eigen::VectorXd grad_minus = finite_difference_gradient(f, x_minus, h); - hess.col(i) = (finite_difference_gradient(f, x, h) - grad_minus) / h; - - x_minus(i) = x(i); - } - - return hess; - } - - /** - * @brief Compute Hessian using central finite differences - * @param f Function to differentiate - * @param x Point at which to evaluate Hessian - * @param h Step size for finite differences (optional) - * @param mode mode for differentiating options: 0 for central, 1 for forward, 2 for backward + * @param mode 0 = central, 1 = forward, 2 = backward * @return Hessian matrix */ template @@ -396,24 +160,52 @@ namespace cddp double h = 2e-5, int mode = 0) { + const int n = x.size(); + Eigen::MatrixXd hess(n, n); + Eigen::VectorXd x_perturbed = x; + if (mode == 0) { - return finite_difference_hessian_central(f, x, h); + // Central differences of gradients + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + Eigen::VectorXd grad_plus = finite_difference_gradient(f, x_perturbed, h); + x_perturbed(i) = x(i) - h; + Eigen::VectorXd grad_minus = finite_difference_gradient(f, x_perturbed, h); + hess.col(i) = (grad_plus - grad_minus) / (2.0 * h); + x_perturbed(i) = x(i); + } } else if (mode == 1) { - return finite_difference_hessian_forward(f, x, h); + // Forward differences of gradients + Eigen::VectorXd grad_x = finite_difference_gradient(f, x, h); + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) + h; + hess.col(i) = (finite_difference_gradient(f, x_perturbed, h) - grad_x) / h; + x_perturbed(i) = x(i); + } } else if (mode == 2) { - return finite_difference_hessian_backward(f, x, h); + // Backward differences of gradients + Eigen::VectorXd grad_x = finite_difference_gradient(f, x, h); + for (int i = 0; i < n; ++i) + { + x_perturbed(i) = x(i) - h; + hess.col(i) = (grad_x - finite_difference_gradient(f, x_perturbed, h)) / h; + x_perturbed(i) = x(i); + } } else { std::cerr << "Invalid mode value for finite difference Hessian" << std::endl; - return Eigen::MatrixXd::Zero(x.size(), x.size()); + return Eigen::MatrixXd::Zero(n, n); } - return Eigen::MatrixXd::Zero(x.size(), x.size()); + + return hess; } // Forward declarations for attitude conversion helper functions @@ -444,4 +236,4 @@ namespace cddp } // namespace helper } // namespace cddp -#endif // CDDP_HELPER_HPP \ No newline at end of file +#endif // CDDP_HELPER_HPP diff --git a/include/cddp-cpp/cddp_core/ipddp_solver.hpp b/include/cddp-cpp/cddp_core/ipddp_solver.hpp index 141aacc1..de3341af 100644 --- a/include/cddp-cpp/cddp_core/ipddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/ipddp_solver.hpp @@ -17,7 +17,7 @@ #ifndef CDDP_IPDDP_SOLVER_HPP #define CDDP_IPDDP_SOLVER_HPP -#include "cddp_core/cddp_core.hpp" +#include "cddp_core/cddp_solver_base.hpp" #include #include #include @@ -29,44 +29,33 @@ namespace cddp /** * @brief Interior Point Differential Dynamic Programming (IPDDP) solver. * - * Implements ISolverAlgorithm interface for inequality constrained problems - * using primal-dual interior point method with logarithmic barrier. + * Inherits from CDDPSolverBase and overrides virtual hooks + * for primal-dual interior point method with logarithmic barrier. */ - class IPDDPSolver : public ISolverAlgorithm + class IPDDPSolver : public CDDPSolverBase { public: - /** - * @brief Default constructor. - */ IPDDPSolver(); - /** - * @brief Initialize solver with CDDP context. - * @param context CDDP instance with problem data and options. - */ void initialize(CDDP &context) override; - - /** - * @brief Execute IPDDP algorithm. - * @param context CDDP instance with problem data and options. - * @return Solution containing trajectories and statistics. - */ - CDDPSolution solve(CDDP &context) override; - - /** - * @brief Get solver name. - * @return "IPDDP" - */ std::string getSolverName() const override; - private: - // Dynamics derivatives - std::vector F_x_; ///< State jacobians - std::vector F_u_; ///< Control jacobians - std::vector> F_xx_; ///< State hessians - std::vector> F_uu_; ///< Control hessians - std::vector> F_ux_; ///< Mixed hessians + protected: + // === CDDPSolverBase virtual hook overrides === + void preIterationSetup(CDDP &context) override; + bool backwardPass(CDDP &context) override; + ForwardPassResult forwardPass(CDDP &context, double alpha) override; + void applyForwardPassResult(CDDP &context, const ForwardPassResult &result) override; + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override; + void postIterationUpdate(CDDP &context, bool forward_pass_success) override; + void recordIterationHistory(const CDDP &context) override; + void populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) override; + void printIteration(int iter, const CDDP &context) const override; + void printSolutionSummary(const CDDPSolution &solution) const override; + private: // Constraint derivatives std::map> G_x_; ///< State gradients std::map> G_u_; ///< Control gradients @@ -77,11 +66,6 @@ namespace cddp std::map> G_ux_; ///< Constraint mixed hessians - // Control law - std::vector k_u_; ///< Feedforward gains - std::vector K_u_; ///< Feedback gains - Eigen::Vector2d dV_; ///< Expected value change - // Interior point variables std::map> G_; ///< Constraint values std::map> Y_; ///< Dual variables @@ -100,151 +84,53 @@ namespace cddp // Pre-allocated workspace for performance optimization struct Workspace { // Backward pass workspace - std::vector A_matrices; ///< A = I + dt*F_x - std::vector B_matrices; ///< B = dt*F_u - std::vector Q_xx_matrices; ///< Q_xx workspace - std::vector Q_ux_matrices; ///< Q_ux workspace - std::vector Q_uu_matrices; ///< Q_uu workspace - std::vector Q_x_vectors; ///< Q_x workspace - std::vector Q_u_vectors; ///< Q_u workspace - + std::vector A_matrices; + std::vector B_matrices; + std::vector Q_xx_matrices; + std::vector Q_ux_matrices; + std::vector Q_uu_matrices; + std::vector Q_x_vectors; + std::vector Q_u_vectors; + // LDLT solver cache - std::vector> ldlt_solvers; ///< Cached LDLT factorizations - std::vector ldlt_valid; ///< Validity flags for LDLT cache - + std::vector> ldlt_solvers; + std::vector ldlt_valid; + // Constraint workspace - Eigen::VectorXd y_combined; ///< Combined dual variables - Eigen::VectorXd s_combined; ///< Combined slack variables - Eigen::VectorXd g_combined; ///< Combined constraint values - Eigen::MatrixXd Q_yu_combined; ///< Combined Q_yu matrix - Eigen::MatrixXd Q_yx_combined; ///< Combined Q_yx matrix - Eigen::MatrixXd YSinv; ///< Y * S^{-1} matrix - Eigen::MatrixXd bigRHS; ///< RHS matrix for solving - + Eigen::VectorXd y_combined; + Eigen::VectorXd s_combined; + Eigen::VectorXd g_combined; + Eigen::MatrixXd Q_yu_combined; + Eigen::MatrixXd Q_yx_combined; + Eigen::MatrixXd YSinv; + Eigen::MatrixXd bigRHS; + // Forward pass workspace - std::vector delta_x_vectors; ///< State deviation vectors - + std::vector delta_x_vectors; + bool initialized = false; } workspace_; - /** - * @brief Precompute dynamics derivatives in parallel. - */ + // === Private helper methods === void precomputeDynamicsDerivatives(CDDP &context); - - /** - * @brief Precompute constraint gradients in parallel. - */ void precomputeConstraintGradients(CDDP &context); - - /** - * @brief Evaluate trajectory cost and constraints. - */ void evaluateTrajectory(CDDP &context); - - /** - * @brief Evaluate trajectory for warm start. - */ void evaluateTrajectoryWarmStart(CDDP &context); - - /** - * @brief Initialize dual/slack variables for warm start. - */ void initializeDualSlackVariablesWarmStart(CDDP &context); - - /** - * @brief Initialize dual/slack variables for cold start. - */ void initializeDualSlackVariables(CDDP &context); - - /** - * @brief Reset barrier parameters and filter. - */ void resetBarrierFilter(CDDP &context); - - /** - * @brief Reset filter for line search. - */ void resetFilter(CDDP &context); - - /** - * @brief Perform backward pass (primal-dual Riccati). - * @return True if successful. - */ - bool backwardPass(CDDP &context); - - /** - * @brief Perform forward pass with line search. - * @return Best result. - */ - ForwardPassResult performForwardPass(CDDP &context); - - /** - * @brief Single forward pass with given step size. - * @param alpha Step size. - * @return Forward pass result. - */ - ForwardPassResult forwardPass(CDDP &context, double alpha); - - /** - * @brief Update barrier parameter. - */ void updateBarrierParameters(CDDP &context, bool forward_pass_success); - - /** - * @brief Get total dual dimension. - */ int getTotalDualDim(const CDDP &context) const; - - /** - * @brief Update iteration history if tracking enabled. - */ - void updateIterationHistory( - const CDDPOptions &options, const CDDP &context, - std::vector &history_objective, std::vector &history_merit_function, - std::vector &history_step_length_primal, std::vector &history_step_length_dual, - std::vector &history_dual_infeasibility, std::vector &history_primal_infeasibility, - std::vector &history_complementary_infeasibility, std::vector &history_barrier_mu, - double alpha_du) const; - - /** - * @brief Check convergence criteria. - * @return True if converged. - */ - bool checkConvergence(const CDDPOptions &options, const CDDP &context, - double dJ, int iter, std::string &termination_reason) const; - - - - /** - * @brief Initialize constraint storage containers. - */ void initializeConstraintStorage(CDDP &context); - - /** - * @brief Compute maximum constraint violation. - */ double computeMaxConstraintViolation(const CDDP &context) const; - - /** - * @brief Compute IPOPT-style scaled dual infeasibility. - * @param context CDDP context with dual/slack variables. - * @return Scaled dual infeasibility metric. - */ double computeScaledDualInfeasibility(const CDDP &context) const; - /** - * @brief Print iteration info (IPOPT style). - */ - void printIteration(int iter, double objective, double inf_pr, double inf_du, - double inf_comp, double mu, double step_norm, double regularization, - double alpha_du, double alpha_pr) const; - - /** - * @brief Print solution summary. - */ - void printSolutionSummary(const CDDPSolution &solution) const; - + // Legacy print helper (used by printIteration override) + void printIterationLegacy(int iter, double objective, double inf_pr, + double inf_du, double inf_comp, double mu, + double step_norm, double regularization, + double alpha_du, double alpha_pr) const; }; } // namespace cddp diff --git a/include/cddp-cpp/cddp_core/logddp_solver.hpp b/include/cddp-cpp/cddp_core/logddp_solver.hpp index 78d7f762..9e8efff7 100644 --- a/include/cddp-cpp/cddp_core/logddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/logddp_solver.hpp @@ -18,7 +18,7 @@ #define CDDP_LOGDDP_SOLVER_HPP #include "cddp_core/barrier.hpp" -#include "cddp_core/cddp_core.hpp" +#include "cddp_core/cddp_solver_base.hpp" #include #include #include @@ -28,140 +28,58 @@ namespace cddp { /** * @brief Log-barrier DDP (LogDDP) solver implementation. * - * This class implements the ISolverAlgorithm interface to provide - * a log-barrier based DDP solver for handling inequality constraints. + * Inherits from CDDPSolverBase (template method pattern) and overrides + * the virtual hooks for log-barrier specific behavior. */ -class LogDDPSolver : public ISolverAlgorithm { +class LogDDPSolver : public CDDPSolverBase { public: - /** - * @brief Default constructor. - */ LogDDPSolver(); - /** - * @brief Initialize the solver with the given CDDP context. - * @param context Reference to the CDDP instance containing problem data and - * options. - */ void initialize(CDDP &context) override; - - /** - * @brief Execute the LogDDP algorithm and return the solution. - * @param context Reference to the CDDP instance containing problem data and - * options. - * @return CDDPSolution containing the results. - */ - CDDPSolution solve(CDDP &context) override; - - /** - * @brief Get the name of the solver algorithm. - * @return String identifier "LogDDP". - */ std::string getSolverName() const override; +protected: + // === Virtual hook overrides === + void preIterationSetup(CDDP &context) override; + bool backwardPass(CDDP &context) override; + ForwardPassResult forwardPass(CDDP &context, double alpha) override; + void applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) override; + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override; + void postIterationUpdate(CDDP &context, bool forward_pass_success) override; + void recordIterationHistory(const CDDP &context) override; + void populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) override; + void printIteration(int iter, const CDDP &context) const override; + private: - // Dynamics storage - std::vector F_; ///< Dynamics evaluations - std::vector F_x_; ///< State jacobians (Fx) - std::vector F_u_; ///< Control jacobians (Fu) - std::vector> F_xx_; ///< State hessians (Fxx) - std::vector> F_uu_; ///< Control hessians (Fuu) - std::vector> F_ux_; ///< Mixed hessians (Fux) - - // Control law parameters - std::vector k_u_; ///< Feedforward control gains - std::vector K_u_; ///< Feedback control gains - Eigen::Vector2d dV_; ///< Expected value function change + // Dynamics storage (forward-simulated trajectory) + std::vector F_; + + // Constraint values g(x,u) - g_ub + std::map> G_; // Log-barrier method - std::map> - G_; ///< Constraint values g(x,u) - g_ub - std::unique_ptr - relaxed_log_barrier_; ///< Log barrier object - double mu_; ///< Barrier parameter - double relaxation_delta_; ///< Relaxation parameter + std::unique_ptr relaxed_log_barrier_; + double mu_; + double relaxation_delta_; // Filter-based line search - double constraint_violation_; ///< Current constraint violation measure + double constraint_violation_; // Multi-shooting parameters - int ms_segment_length_; ///< Multi-shooting segment length - - /** - * @brief Pre-compute dynamics jacobians and hessians for all time steps in - * parallel. - * @param context Reference to the CDDP context. - */ - void precomputeDynamicsDerivatives(CDDP &context); - - /** - * @brief Efficiently compute only jacobians using execution policies. - * @param context Reference to the CDDP context. - */ - void precomputeJacobiansOnly(CDDP &context); - - /** - * @brief Cache-friendly sequential computation with better memory access - * patterns. - * @param context Reference to the CDDP context. - */ - void precomputeDynamicsDerivativesOptimized(CDDP &context); + int ms_segment_length_; /** * @brief Evaluate trajectory by computing cost, dynamics, and merit function. - * @param context Reference to the CDDP context. */ void evaluateTrajectory(CDDP &context); /** * @brief Reset/initialize the filter for line search. - * @param context Reference to the CDDP context. */ void resetFilter(CDDP &context); - - /** - * @brief Perform backward pass (Riccati recursion with log-barrier terms). - * @param context Reference to the CDDP context. - * @return True if backward pass succeeds, false otherwise. - */ - bool backwardPass(CDDP &context); - - /** - * @brief Perform forward pass with line search. - * @param context Reference to the CDDP context. - * @return Best forward pass result. - */ - ForwardPassResult performForwardPass(CDDP &context); - - /** - * @brief Perform single forward pass with given step size. - * @param context Reference to the CDDP context. - * @param alpha Step size for the forward pass. - * @return Forward pass result. - */ - ForwardPassResult forwardPass(CDDP &context, double alpha); - - /** - * @brief Update barrier parameters. - * @param context Reference to the CDDP context. - * @param forward_pass_success Whether the forward pass was successful. - * @param termination_metric Current termination metric. - */ - void updateBarrierParameters(CDDP &context, bool forward_pass_success, - double termination_metric); - - /** - * @brief Print iteration information. - */ - void printIteration(int iter, double cost, double lagrangian, double opt_gap, - double regularization, double alpha, double mu, - double constraint_violation) const; - - /** - * @brief Print solution summary. - * @param solution The solution to print. - */ - void printSolutionSummary(const CDDPSolution &solution) const; }; } // namespace cddp diff --git a/include/cddp-cpp/cddp_core/msipddp_solver.hpp b/include/cddp-cpp/cddp_core/msipddp_solver.hpp index 7e044e0b..30eb218d 100644 --- a/include/cddp-cpp/cddp_core/msipddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/msipddp_solver.hpp @@ -17,7 +17,7 @@ #ifndef CDDP_MSIPDDP_SOLVER_HPP #define CDDP_MSIPDDP_SOLVER_HPP -#include "cddp_core/cddp_core.hpp" +#include "cddp_core/cddp_solver_base.hpp" #include #include #include @@ -27,265 +27,121 @@ namespace cddp { /** - * @brief Interior Point Differential Dynamic Programming (MSIPDDP) solver. + * @brief Multiple-Shooting Interior Point DDP (MSIPDDP) solver. * - * Implements ISolverAlgorithm interface for inequality constrained problems - * using primal-dual interior point method with logarithmic barrier. + * Inherits from CDDPSolverBase and overrides virtual hooks + * for primal-dual interior point method with multi-shooting. */ - class MSIPDDPSolver : public ISolverAlgorithm + class MSIPDDPSolver : public CDDPSolverBase { public: - /** - * @brief Default constructor. - */ MSIPDDPSolver(); - /** - * @brief Initialize solver with CDDP context. - * @param context CDDP instance with problem data and options. - */ void initialize(CDDP &context) override; - - /** - * @brief Execute MSIPDDP algorithm. - * @param context CDDP instance with problem data and options. - * @return Solution containing trajectories and statistics. - */ - CDDPSolution solve(CDDP &context) override; - - /** - * @brief Get solver name. - * @return "MSIPDDP" - */ std::string getSolverName() const override; + protected: + // === CDDPSolverBase virtual hook overrides === + void preIterationSetup(CDDP &context) override; + bool backwardPass(CDDP &context) override; + ForwardPassResult forwardPass(CDDP &context, double alpha) override; + void applyForwardPassResult(CDDP &context, const ForwardPassResult &result) override; + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override; + void postIterationUpdate(CDDP &context, bool forward_pass_success) override; + bool handleForwardPassFailure(CDDP &context, std::string &termination_reason) override; + void recordIterationHistory(const CDDP &context) override; + void populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) override; + void printIteration(int iter, const CDDP &context) const override; + void printSolutionSummary(const CDDPSolution &solution) const override; + private: - // Dynamics storage + // Dynamics storage (multi-shooting) std::vector F_; ///< Dynamics evaluations - std::vector F_x_; ///< State jacobians - std::vector F_u_; ///< Control jacobians - std::vector> F_xx_; ///< State hessians - std::vector> F_uu_; ///< Control hessians - std::vector> F_ux_; ///< Mixed hessians // Constraint derivatives - std::map> G_x_; ///< State gradients - std::map> G_u_; ///< Control gradients - std::map>> - G_xx_; ///< Constraint state hessians (time x dual_dim) - std::map>> - G_uu_; ///< Constraint control hessians (time x dual_dim) - std::map>> - G_ux_; ///< Constraint mixed hessians (time x dual_dim) - - // Control law - std::vector k_u_; ///< Feedforward gains - std::vector K_u_; ///< Feedback gains - Eigen::Vector2d dV_; ///< Expected value change + std::map> G_x_; + std::map> G_u_; + std::map>> G_xx_; + std::map>> G_uu_; + std::map>> G_ux_; // Interior point variables - std::map> G_; ///< Constraint values - std::map> Y_; ///< Dual variables - std::map> S_; ///< Slack variables + std::map> G_; + std::map> Y_; + std::map> S_; // Interior point gains - std::map> k_y_; ///< Dual feedforward - std::map> K_y_; ///< Dual feedback - std::map> k_s_; ///< Slack feedforward - std::map> K_s_; ///< Slack feedback + std::map> k_y_; + std::map> K_y_; + std::map> k_s_; + std::map> K_s_; // MSIPDDP-specific costate variables and gains - std::vector - Lambda_; ///< Costate variables (Lagrange multipliers for dynamics) - std::vector - k_lambda_; ///< Feedforward gains for costate variables - std::vector - K_lambda_; ///< Feedback gains for costate variables - + std::vector Lambda_; + std::vector k_lambda_; + std::vector K_lambda_; + // Multi-shooting specific parameters - int ms_segment_length_; ///< Length of shooting segments + int ms_segment_length_; // Barrier method parameters - double mu_; ///< Barrier parameter - std::vector filter_; ///< Filter for line search + double mu_; + std::vector filter_; - // Pre-allocated workspace for performance optimization + // Pre-allocated workspace struct Workspace { - // Backward pass workspace - std::vector A_matrices; ///< A = I + dt*F_x - std::vector B_matrices; ///< B = dt*F_u - std::vector Q_xx_matrices; ///< Q_xx workspace - std::vector Q_ux_matrices; ///< Q_ux workspace - std::vector Q_uu_matrices; ///< Q_uu workspace - std::vector Q_x_vectors; ///< Q_x workspace - std::vector Q_u_vectors; ///< Q_u workspace - - // LDLT solver cache - std::vector> ldlt_solvers; ///< Cached LDLT factorizations - std::vector ldlt_valid; ///< Validity flags for LDLT cache - - // Constraint workspace - Eigen::VectorXd y_combined; ///< Combined dual variables - Eigen::VectorXd s_combined; ///< Combined slack variables - Eigen::VectorXd g_combined; ///< Combined constraint values - Eigen::MatrixXd Q_yu_combined; ///< Combined Q_yu matrix - Eigen::MatrixXd Q_yx_combined; ///< Combined Q_yx matrix - Eigen::MatrixXd YSinv; ///< Y * S^{-1} matrix - Eigen::MatrixXd bigRHS; ///< RHS matrix for solving - - // Forward pass workspace - std::vector delta_x_vectors; ///< State deviation vectors - - // MSIPDDP-specific workspace - std::vector d_vectors; ///< Defect vectors - + std::vector A_matrices; + std::vector B_matrices; + std::vector Q_xx_matrices; + std::vector Q_ux_matrices; + std::vector Q_uu_matrices; + std::vector Q_x_vectors; + std::vector Q_u_vectors; + + std::vector> ldlt_solvers; + std::vector ldlt_valid; + + Eigen::VectorXd y_combined; + Eigen::VectorXd s_combined; + Eigen::VectorXd g_combined; + Eigen::MatrixXd Q_yu_combined; + Eigen::MatrixXd Q_yx_combined; + Eigen::MatrixXd YSinv; + Eigen::MatrixXd bigRHS; + + std::vector delta_x_vectors; + std::vector d_vectors; ///< Defect vectors + bool initialized = false; } workspace_; - /** - * @brief Precompute dynamics derivatives in parallel. - */ + // === Private helper methods === void precomputeDynamicsDerivatives(CDDP &context); - - /** - * @brief Precompute constraint gradients in parallel. - */ void precomputeConstraintGradients(CDDP &context); - - /** - * @brief Evaluate trajectory cost and constraints. - */ void evaluateTrajectory(CDDP &context); - - /** - * @brief Evaluate trajectory for warm start. - */ void evaluateTrajectoryWarmStart(CDDP &context); - - /** - * @brief Initialize dual/slack/costate variables for warm start. - */ void initializeDualSlackCostateVariablesWarmStart(CDDP &context); - - /** - * @brief Initialize dual/slack/costate variables for cold start. - */ void initializeDualSlackCostateVariables(CDDP &context); - - /** - * @brief Reset barrier parameters and filter. - */ void resetBarrierFilter(CDDP &context); - - /** - * @brief Reset filter for line search. - */ void resetFilter(CDDP &context); - - /** - * @brief Accept new filter entry with domination check. - * @param merit_function Merit function value - * @param constraint_violation Constraint violation measure - * @return true if entry was accepted, false otherwise - */ bool acceptFilterEntry(double merit_function, double constraint_violation); - - /** - * @brief Check if candidate point is acceptable to filter. - * @param merit_function Merit function value - * @param constraint_violation Constraint violation measure - * @param options Filter options - * @param expected_improvement Expected improvement from model - * @return true if point is acceptable, false otherwise - */ bool isFilterAcceptable(double merit_function, double constraint_violation, const SolverSpecificFilterOptions &options, double expected_improvement) const; - - /** - * @brief Check if filter needs restoration and perform if necessary. - * @param context CDDP context - * @return true if restoration was successful, false otherwise - */ bool checkAndPerformFilterRestoration(CDDP &context); - - /** - * @brief Perform backward pass (primal-dual Riccati). - * @return True if successful. - */ - bool backwardPass(CDDP &context); - - /** - * @brief Perform forward pass with line search. - * @return Best result. - */ - ForwardPassResult performForwardPass(CDDP &context); - - /** - * @brief Single forward pass with given step size. - * @param alpha Step size. - * @return Forward pass result. - */ - ForwardPassResult forwardPass(CDDP &context, double alpha); - - /** - * @brief Update barrier parameter. - */ void updateBarrierParameters(CDDP &context, bool forward_pass_success); - - /** - * @brief Get total dual dimension. - */ int getTotalDualDim(const CDDP &context) const; - - /** - * @brief Update iteration history if tracking enabled. - */ - void updateIterationHistory( - const CDDPOptions &options, const CDDP &context, - std::vector &history_objective, std::vector &history_merit_function, - std::vector &history_step_length_primal, std::vector &history_step_length_dual, - std::vector &history_dual_infeasibility, std::vector &history_primal_infeasibility, - std::vector &history_complementary_infeasibility, std::vector &history_barrier_mu, - double alpha_du) const; - - /** - * @brief Check convergence criteria. - * @return True if converged. - */ - bool checkConvergence(const CDDPOptions &options, const CDDP &context, - double dJ, int iter, std::string &termination_reason) const; - - - - /** - * @brief Initialize constraint storage containers. - */ void initializeConstraintStorage(CDDP &context); - - /** - * @brief Compute maximum constraint violation. - */ double computeMaxConstraintViolation(const CDDP &context) const; - - /** - * @brief Compute IPOPT-style scaled dual infeasibility. - * @param context CDDP context with dual/slack variables. - * @return Scaled dual infeasibility metric. - */ double computeScaledDualInfeasibility(const CDDP &context) const; - /** - * @brief Print iteration info (IPOPT style). - */ - void printIteration(int iter, double objective, double inf_pr, double inf_du, - double inf_comp, double mu, double step_norm, double regularization, - double alpha_du, double alpha_pr) const; - - /** - * @brief Print solution summary. - */ - void printSolutionSummary(const CDDPSolution &solution) const; + // Legacy print helper + void printIterationLegacy(int iter, double objective, double inf_pr, + double inf_du, double inf_comp, double mu, + double step_norm, double regularization, + double alpha_du, double alpha_pr) const; }; } // namespace cddp diff --git a/include/cddp-cpp/cddp_core/options.hpp b/include/cddp-cpp/cddp_core/options.hpp index 3e6ff6e1..cee6bb9c 100644 --- a/include/cddp-cpp/cddp_core/options.hpp +++ b/include/cddp-cpp/cddp_core/options.hpp @@ -105,94 +105,63 @@ namespace cddp }; /** - * @brief Options for the relaxed log-barrier method. - * This struct will be instantiated within each solver's specific options if it - * uses a relaxed log-barrier method. + * @brief Common options for interior-point solvers (dual/slack initialization + barrier). */ - struct LogBarrierOptions + struct InteriorPointOptions { - bool use_relaxed_log_barrier_penalty = - false; ///< Use relaxed log-barrier method (if applicable to the solver). - double relaxed_log_barrier_delta = - 1e-10; ///< Relaxation delta for relaxed log-barrier (if applicable). + double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables. + double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables. + SolverSpecificBarrierOptions barrier; ///< Barrier method parameters. + }; - // Multi-shooting specific + /** + * @brief Common options for multi-shooting solvers. + */ + struct MultiShootingOptions + { int segment_length = 5; ///< Number of shooting intervals before a gap-closing constraint. std::string rollout_type = "nonlinear"; ///< Rollout type: "nonlinear", "hybrid". bool use_controlled_rollout = - true; ///< Use controlled rollout (propagates x_{k+1} = f(x_k, u_k) during - ///< initial rollout). - - SolverSpecificBarrierOptions - barrier; ///< Barrier method parameters for relaxed log-barrier. + false; ///< Use controlled rollout during initial rollout. + double costate_var_init_scale = + 1e-6; ///< Initial scale for costate variables. }; /** - * @brief Comprehensive options specifically for the IPDDP (Interior-Point - * Differential Dynamic Programming) algorithm. Includes its own instances of - * barrier and filter parameters. + * @brief Options for the relaxed log-barrier method. */ - struct IPDDPAlgorithmOptions + struct LogBarrierOptions : MultiShootingOptions { - // Initialization scales for duals/slacks - double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables. - double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables. - + LogBarrierOptions() { use_controlled_rollout = true; } + bool use_relaxed_log_barrier_penalty = + false; ///< Use relaxed log-barrier method. + double relaxed_log_barrier_delta = + 1e-10; ///< Relaxation delta for relaxed log-barrier. SolverSpecificBarrierOptions - barrier; ///< Barrier method parameters for IPDDP. + barrier; ///< Barrier method parameters for relaxed log-barrier. }; /** - * @brief Comprehensive options specifically for the MSIPDDP (Multi-Shooting - * Interior-Point Differential Dynamic Programming) algorithm. Includes its own - * instances of barrier and filter parameters, plus multi-shooting specifics. + * @brief Options for the IPDDP solver. */ - struct MSIPDDPAlgorithmOptions - { - // Initialization scales - double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables. - double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables. - double costate_var_init_scale = - 1e-6; ///< Initial scale for costate variables. - - // Multi-shooting specific - int segment_length = - 5; ///< Number of shooting intervals before a gap-closing constraint. - std::string rollout_type = - "nonlinear"; ///< Rollout type: "nonlinear", "hybrid". - bool use_controlled_rollout = - false; ///< Use controlled rollout (propagates x_{k+1} = f(x_k, u_k) - ///< during initial rollout). + struct IPDDPAlgorithmOptions : InteriorPointOptions {}; - SolverSpecificBarrierOptions - barrier; ///< Barrier method parameters for MSIPDDP.. - }; + /** + * @brief Options for the MSIPDDP solver (interior-point + multi-shooting). + */ + struct MSIPDDPAlgorithmOptions : InteriorPointOptions, MultiShootingOptions {}; /** - * @brief Options specifically for TC-MSIPDDP algorithm. (Placeholder) + * @brief Options for the TC-MSIPDDP solver (terminal-constrained multi-shooting). */ - struct TCMSIPDDPAlgorithmOptions + struct TCMSIPDDPAlgorithmOptions : InteriorPointOptions, MultiShootingOptions { - // Initialization scales - double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables - double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables - double costate_var_init_scale = 1e-6; ///< Initial scale for costate variables - - // Terminal constraint specific - double terminal_dual_init_scale = 1e-1; ///< Initial scale for terminal dual variables - double terminal_slack_init_scale = 1e-2; ///< Initial scale for terminal slack variables - double terminal_constraint_tolerance = 1e-6; ///< Tolerance for terminal constraint satisfaction - - // Multi-shooting specific - int segment_length = 5; ///< Number of shooting intervals - std::string rollout_type = "nonlinear"; ///< Rollout type: "nonlinear", "hybrid" - bool use_controlled_rollout = false; ///< Use controlled rollout - - // Barrier and filter options - SolverSpecificBarrierOptions barrier; ///< Barrier method parameters + double terminal_dual_init_scale = 1e-1; ///< Initial scale for terminal dual variables. + double terminal_slack_init_scale = 1e-2; ///< Initial scale for terminal slack variables. + double terminal_constraint_tolerance = 1e-6; ///< Tolerance for terminal constraint satisfaction. }; /** diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp new file mode 100644 index 00000000..1f282a8e --- /dev/null +++ b/src/cddp_core/cddp_solver_base.cpp @@ -0,0 +1,411 @@ +/* + Copyright 2025 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/cddp_solver_base.hpp" +#include +#include +#include +#include +#include + +namespace cddp { + +// === IterationHistory === + +void CDDPSolverBase::IterationHistory::reserve(size_t n) { + objective.reserve(n); + merit_function.reserve(n); + step_length_primal.reserve(n); + step_length_dual.reserve(n); + dual_infeasibility.reserve(n); + primal_infeasibility.reserve(n); + complementary_infeasibility.reserve(n); + barrier_mu.reserve(n); + regularization.reserve(n); +} + +void CDDPSolverBase::IterationHistory::clear() { + objective.clear(); + merit_function.clear(); + step_length_primal.clear(); + step_length_dual.clear(); + dual_infeasibility.clear(); + primal_infeasibility.clear(); + complementary_infeasibility.clear(); + barrier_mu.clear(); + regularization.clear(); +} + +// === Template method: solve() === + +CDDPSolution CDDPSolverBase::solve(CDDP &context) { + const CDDPOptions &options = context.getOptions(); + + // Print header/options if requested + if (options.print_solver_header) { + context.printSolverInfo(); + } + if (options.print_solver_options) { + context.printOptions(options); + } + + // Prepare solution + CDDPSolution solution; + solution["solver_name"] = getSolverName(); + solution["status_message"] = std::string("Running"); + solution["iterations_completed"] = 0; + solution["solve_time_ms"] = 0.0; + + // Initialize history + if (options.return_iteration_info) { + history_.clear(); + history_.reserve(static_cast(options.max_iterations + 1)); + } + + // Solver-specific pre-loop setup (evaluateTrajectory, resetFilter, etc.) + preIterationSetup(context); + + // Record initial state + if (options.return_iteration_info) { + recordIterationHistory(context); + } + + if (options.verbose) { + printIteration(0, context); + } + + // Start timer + auto start_time = std::chrono::high_resolution_clock::now(); + int iter = 0; + bool converged = false; + std::string termination_reason = "MaxIterationsReached"; + double dJ = 0.0; + double dL = 0.0; + + // Main iteration loop + while (iter < options.max_iterations) { + ++iter; + + // Check CPU time limit + if (options.max_cpu_time > 0) { + auto elapsed = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start_time); + if (elapsed.count() > options.max_cpu_time * 1000) { + termination_reason = "MaxCpuTimeReached"; + if (options.verbose) { + std::cerr << getSolverName() + << ": Maximum CPU time reached. Returning current solution" + << std::endl; + } + break; + } + } + + // Backward pass with regularization retry + bool backward_ok = false; + while (!backward_ok) { + backward_ok = backwardPass(context); + if (!backward_ok) { + context.increaseRegularization(); + if (context.isRegularizationLimitReached()) { + termination_reason = "RegularizationLimitReached_NotConverged"; + if (options.verbose) { + std::cerr << getSolverName() + << ": Backward pass regularization limit reached" + << std::endl; + } + break; + } + } + } + if (!backward_ok) + break; + + // Forward pass with line search + ForwardPassResult best_result = performForwardPass(context); + + if (best_result.success) { + dJ = context.cost_ - best_result.cost; + dL = context.merit_function_ - best_result.merit_function; + + applyForwardPassResult(context, best_result); + + if (options.return_iteration_info) { + recordIterationHistory(context); + } + + context.decreaseRegularization(); + } else { + // Handle failure (default: increase regularization; MSIPDDP: try filter + // restoration first) + bool should_break = handleForwardPassFailure(context, termination_reason); + if (should_break) + break; + } + + // Check convergence + converged = checkConvergence(context, dJ, dL, iter, termination_reason); + + if (options.verbose) { + printIteration(iter, context); + } + + if (converged) + break; + + // Post-iteration update (barrier parameter update, etc.) + postIterationUpdate(context, best_result.success); + } + + // Compute final timing + auto end_time = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast( + end_time - start_time); + + // Populate common solution fields + solution["status_message"] = termination_reason; + solution["iterations_completed"] = iter; + solution["solve_time_ms"] = static_cast(duration.count()); + solution["final_objective"] = context.cost_; + solution["final_step_length"] = context.alpha_pr_; + solution["time_points"] = buildTimePoints(context); + solution["state_trajectory"] = context.X_; + solution["control_trajectory"] = context.U_; + solution["control_feedback_gains_K"] = K_u_; + solution["final_regularization"] = context.regularization_; + + // Add iteration history if requested + if (options.return_iteration_info) { + solution["history_objective"] = history_.objective; + solution["history_merit_function"] = history_.merit_function; + solution["history_step_length_primal"] = history_.step_length_primal; + solution["history_step_length_dual"] = history_.step_length_dual; + solution["history_dual_infeasibility"] = history_.dual_infeasibility; + solution["history_primal_infeasibility"] = history_.primal_infeasibility; + solution["history_complementary_infeasibility"] = + history_.complementary_infeasibility; + solution["history_barrier_mu"] = history_.barrier_mu; + std::map> history_regularization_map; + history_regularization_map["control"] = history_.regularization; + solution["history_regularization"] = history_regularization_map; + } + + // Solver-specific solution fields + populateSolverSpecificSolution(solution, context); + + if (options.verbose) { + printSolutionSummary(solution); + } + + return solution; +} + +// === Default implementations === + +void CDDPSolverBase::applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) { + context.X_ = result.state_trajectory; + context.U_ = result.control_trajectory; + context.cost_ = result.cost; + context.merit_function_ = result.merit_function; + context.alpha_pr_ = result.alpha_pr; + context.alpha_du_ = result.alpha_du; +} + +bool CDDPSolverBase::handleForwardPassFailure(CDDP &context, + std::string &termination_reason) { + context.increaseRegularization(); + if (context.isRegularizationLimitReached()) { + termination_reason = "RegularizationLimitReached_NotConverged"; + if (context.getOptions().verbose) { + std::cerr << getSolverName() + << ": Regularization limit reached. Not converged." << std::endl; + } + return true; // break + } + return false; // continue +} + +void CDDPSolverBase::recordIterationHistory(const CDDP &context) { + history_.objective.push_back(context.cost_); + history_.merit_function.push_back(context.merit_function_); + history_.step_length_primal.push_back(context.alpha_pr_); + history_.step_length_dual.push_back(context.alpha_du_); + history_.dual_infeasibility.push_back(context.inf_du_); + history_.primal_infeasibility.push_back(context.inf_pr_); + history_.complementary_infeasibility.push_back(context.inf_comp_); + history_.regularization.push_back(context.regularization_); +} + +void CDDPSolverBase::printSolutionSummary(const CDDPSolution &solution) const { + std::cout << "\n========================================\n"; + std::cout << " " << getSolverName() << " Solution Summary\n"; + std::cout << "========================================\n"; + + auto iterations = std::any_cast(solution.at("iterations_completed")); + auto solve_time = std::any_cast(solution.at("solve_time_ms")); + auto final_cost = std::any_cast(solution.at("final_objective")); + auto status = std::any_cast(solution.at("status_message")); + + std::cout << "Status: " << status << "\n"; + std::cout << "Iterations: " << iterations << "\n"; + std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; + std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; + std::cout << "========================================\n\n"; +} + +// === Shared utilities === + +ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { + const CDDPOptions &options = context.getOptions(); + ForwardPassResult best_result; + best_result.cost = std::numeric_limits::infinity(); + best_result.merit_function = std::numeric_limits::infinity(); + best_result.success = false; + + if (!options.enable_parallel) { + // Single-threaded with early termination + for (double alpha : context.alphas_) { + ForwardPassResult result = forwardPass(context, alpha); + if (result.success) { + best_result = result; + break; + } + } + } else { + // Multi-threaded + std::vector> futures; + futures.reserve(context.alphas_.size()); + + for (double alpha : context.alphas_) { + futures.push_back( + std::async(std::launch::async, + [this, &context, alpha]() { return forwardPass(context, alpha); })); + } + + for (auto &future : futures) { + try { + if (future.valid()) { + ForwardPassResult result = future.get(); + if (result.success && + result.merit_function < best_result.merit_function) { + best_result = result; + } + } + } catch (const std::exception &e) { + if (options.verbose) { + std::cerr << getSolverName() + << ": Forward pass thread failed: " << e.what() + << std::endl; + } + } + } + } + + return best_result; +} + +void CDDPSolverBase::precomputeDynamicsDerivatives( + CDDP &context, int min_horizon_for_parallel) { + const CDDPOptions &options = context.getOptions(); + const int horizon = context.getHorizon(); + const int state_dim = context.getStateDim(); + const int control_dim = context.getControlDim(); + + // Resize storage + F_x_.resize(horizon); + F_u_.resize(horizon); + if (!options.use_ilqr) { + F_xx_.resize(horizon); + F_uu_.resize(horizon); + F_ux_.resize(horizon); + } + + auto compute_derivatives = [&](int t) { + const Eigen::VectorXd &x = context.X_[t]; + const Eigen::VectorXd &u = context.U_[t]; + double time = t * context.getTimestep(); + + auto [Fx, Fu] = context.getSystem().getJacobians(x, u, time); + // Convert to discrete time + F_x_[t] = context.getTimestep() * Fx; + F_x_[t].diagonal().array() += 1.0; + F_u_[t] = context.getTimestep() * Fu; + + if (!options.use_ilqr) { + auto [Fxx, Fuu, Fux] = context.getSystem().getHessians(x, u, time); + F_xx_[t].resize(state_dim); + F_uu_[t].resize(state_dim); + F_ux_[t].resize(state_dim); + for (int i = 0; i < state_dim; ++i) { + F_xx_[t][i] = context.getTimestep() * Fxx[i]; + F_uu_[t][i] = context.getTimestep() * Fuu[i]; + F_ux_[t][i] = context.getTimestep() * Fux[i]; + } + } + }; + + bool use_parallel = + options.enable_parallel && horizon >= min_horizon_for_parallel; + + if (use_parallel) { + std::vector> futures; + futures.reserve(horizon); + for (int t = 0; t < horizon; ++t) { + futures.push_back( + std::async(std::launch::async, compute_derivatives, t)); + } + for (auto &f : futures) { + f.get(); + } + } else { + for (int t = 0; t < horizon; ++t) { + compute_derivatives(t); + } + } +} + +void CDDPSolverBase::initializeGains(int horizon, int control_dim, + int state_dim) { + k_u_.resize(horizon); + K_u_.resize(horizon); + for (int t = 0; t < horizon; ++t) { + k_u_[t] = Eigen::VectorXd::Zero(control_dim); + K_u_[t] = Eigen::MatrixXd::Zero(control_dim, state_dim); + } + dV_ = Eigen::Vector2d::Zero(); +} + +std::vector CDDPSolverBase::buildTimePoints(const CDDP &context) { + std::vector time_points; + time_points.reserve(static_cast(context.getHorizon() + 1)); + for (int t = 0; t <= context.getHorizon(); ++t) { + time_points.push_back(t * context.getTimestep()); + } + return time_points; +} + +void CDDPSolverBase::computeCost(CDDP &context) { + context.cost_ = 0.0; + for (int t = 0; t < context.getHorizon(); ++t) { + context.cost_ += + context.getObjective().running_cost(context.X_[t], context.U_[t], t); + } + context.cost_ += context.getObjective().terminal_cost(context.X_.back()); + context.merit_function_ = context.cost_; +} + +} // namespace cddp diff --git a/src/cddp_core/clddp_solver.cpp b/src/cddp_core/clddp_solver.cpp index a01d897f..51176554 100644 --- a/src/cddp_core/clddp_solver.cpp +++ b/src/cddp_core/clddp_solver.cpp @@ -17,10 +17,7 @@ #include "cddp_core/clddp_solver.hpp" #include "cddp_core/boxqp.hpp" #include "cddp_core/cddp_core.hpp" -#include #include -#include -#include #include #include @@ -30,19 +27,16 @@ CLDDPSolver::CLDDPSolver() : boxqp_solver_(BoxQPOptions()) {} void CLDDPSolver::initialize(CDDP &context) { const CDDPOptions &options = context.getOptions(); - int horizon = context.getHorizon(); int control_dim = context.getControlDim(); int state_dim = context.getStateDim(); - // For warm starts, verify that existing state is valid + // Warm start validation if (options.warm_start) { - // Check if solver state is properly initialized and compatible bool valid_warm_start = (k_u_.size() == static_cast(horizon) && K_u_.size() == static_cast(horizon)); if (valid_warm_start && !k_u_.empty()) { - // Verify dimensions are consistent for (int t = 0; t < horizon; ++t) { if (k_u_[t].size() != control_dim || K_u_[t].rows() != control_dim || K_u_[t].cols() != state_dim) { @@ -55,256 +49,31 @@ void CLDDPSolver::initialize(CDDP &context) { } if (valid_warm_start) { - // Valid warm start: only update what's necessary if (options.verbose) { std::cout << "CLDDP: Using warm start with existing control gains" << std::endl; } - - // Only update BoxQP solver options if they changed boxqp_solver_.setOptions(options.box_qp); - - // Compute cost for current trajectories if (!context.X_.empty() && !context.U_.empty()) { computeCost(context); } - - // Keep existing k_u_, K_u_, and other solver state return; - } else { - // Invalid warm start: fall back to cold start with warning - if (options.verbose) { - std::cout << "CLDDP: Warning - warm start requested but no valid " - "solver state found. " - << "Falling back to cold start initialization." << std::endl; - } + } else if (options.verbose) { + std::cout << "CLDDP: Warning - warm start requested but no valid " + "solver state found. Falling back to cold start." + << std::endl; } } - // Cold start: full initialization (also used as fallback for invalid warm - // start) - k_u_.resize(horizon); - K_u_.resize(horizon); - for (int t = 0; t < horizon; ++t) { - k_u_[t] = Eigen::VectorXd::Zero(control_dim); - K_u_[t] = Eigen::MatrixXd::Zero(control_dim, state_dim); - } - - dV_ = Eigen::Vector2d::Zero(); - - // Setup BoxQP solver + // Cold start + initializeGains(horizon, control_dim, state_dim); boxqp_solver_ = BoxQPSolver(options.box_qp); - // Compute initial cost if trajectories exist if (!context.X_.empty() && !context.U_.empty()) { computeCost(context); } } -CDDPSolution CLDDPSolver::solve(CDDP &context) { - const CDDPOptions &options = context.getOptions(); - - // Print solver header if requested - if (options.print_solver_header) { - context.printSolverInfo(); - } - - // Print solver options if requested - if (options.print_solver_options) { - context.printOptions(options); - } - - // Prepare solution map - CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("Running"); - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; - - // Initialize history vectors only if requested - std::vector history_objective; - std::vector history_merit_function; - std::vector history_step_length_primal; - std::vector history_dual_infeasibility; - std::vector history_regularization; - - if (options.return_iteration_info) { - // Pre-allocate history vectors for efficiency (max_iterations + 1 for - // initial iteration) - const size_t expected_size = - static_cast(options.max_iterations + 1); - history_objective.reserve(expected_size); - history_merit_function.reserve(expected_size); - history_step_length_primal.reserve(expected_size); - history_dual_infeasibility.reserve(expected_size); - history_regularization.reserve(expected_size); - - // Initial iteration values - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_dual_infeasibility.push_back(context.inf_du_); - history_regularization.push_back(context.regularization_); - } - - if (options.verbose) { - printIteration(0, context.cost_, context.merit_function_, context.inf_du_, - context.regularization_, context.alpha_pr_); - } - - // Start timer - auto start_time = std::chrono::high_resolution_clock::now(); - int iter = 0; - bool converged = false; - std::string termination_reason = "MaxIterationsReached"; // Default assumption - - // Main CLDDP loop - while (iter < options.max_iterations) { - ++iter; - - // Check maximum CPU time - if (options.max_cpu_time > 0) { - auto current_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - current_time - start_time); - if (duration.count() > options.max_cpu_time * 1000) { - termination_reason = "MaxCpuTimeReached"; - if (options.verbose) { - std::cerr - << "CLDDP: Maximum CPU time reached. Returning current solution" - << std::endl; - } - break; - } - } - - // 1. Backward pass - bool backward_pass_success = false; - while (!backward_pass_success) { - backward_pass_success = backwardPass(context); - - if (!backward_pass_success) { - context.increaseRegularization(); - if (context.isRegularizationLimitReached()) { - termination_reason = "RegularizationLimit_NotConverged"; - if (options.verbose) { - std::cerr << "CLDDP: Backward pass regularization limit reached" - << std::endl; - } - break; - } - } - } - - if (!backward_pass_success) - break; - - // Check convergence - if (context.inf_du_ < options.tolerance) { - converged = true; - termination_reason = "OptimalSolutionFound"; - break; - } - - // 2. Forward pass - ForwardPassResult best_result = performForwardPass(context); - - // Update solution if forward pass succeeded - if (best_result.success) { - context.X_ = best_result.state_trajectory; - context.U_ = best_result.control_trajectory; - double dJ = context.cost_ - best_result.cost; - context.cost_ = best_result.cost; - context.merit_function_ = best_result.merit_function; - context.alpha_pr_ = best_result.alpha_pr; - - // Store history only if requested - if (options.return_iteration_info) { - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_step_length_primal.push_back( - context.getCurrentPrimalStepSize()); - history_dual_infeasibility.push_back(context.inf_du_); - history_regularization.push_back(context.regularization_); - } - - context.decreaseRegularization(); - - // Check convergence - if (dJ < options.acceptable_tolerance) { - converged = true; - termination_reason = "AcceptableSolutionFound"; - break; - } - } else { - context.increaseRegularization(); - - // Check if regularization limit reached - if (context.isRegularizationLimitReached()) { - termination_reason = "RegularizationLimitReached_NotConverged"; - converged = false; - if (options.verbose) { - std::cerr << "CLDDP: Regularization limit reached. Not converged." - << std::endl; - } - break; - } - } - - // Print iteration info - if (options.verbose) { - printIteration(iter, context.cost_, context.merit_function_, - context.inf_du_, context.regularization_, - context.alpha_pr_); - } - } - - // Compute final timing - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - end_time - start_time); - - // Populate final solution with detailed status - solution["status_message"] = termination_reason; - solution["iterations_completed"] = iter; - solution["solve_time_ms"] = static_cast(duration.count()); - solution["final_objective"] = context.cost_; - solution["final_step_length"] = context.alpha_pr_; - - // Add trajectories - std::vector time_points; - time_points.reserve(static_cast(context.getHorizon() + - 1)); // Pre-allocate for efficiency - for (int t = 0; t <= context.getHorizon(); ++t) { - time_points.push_back(t * context.getTimestep()); - } - solution["time_points"] = time_points; - solution["state_trajectory"] = context.X_; - solution["control_trajectory"] = context.U_; - - // Add iteration history if requested - if (options.return_iteration_info) { - solution["history_objective"] = history_objective; - solution["history_merit_function"] = history_merit_function; - solution["history_step_length_primal"] = history_step_length_primal; - solution["history_dual_infeasibility"] = history_dual_infeasibility; - std::map> history_regularization_map; - history_regularization_map["control"] = history_regularization; - solution["history_regularization"] = history_regularization_map; - } - - // Add control gains - solution["control_feedback_gains_K"] = K_u_; - - // Final metrics - solution["final_regularization"] = context.regularization_; - - if (options.verbose) { - printSolutionSummary(solution); - } - - return solution; -} - std::string CLDDPSolver::getSolverName() const { return "CLDDP"; } bool CLDDPSolver::backwardPass(CDDP &context) { @@ -313,21 +82,19 @@ bool CLDDPSolver::backwardPass(CDDP &context) { const int control_dim = context.getControlDim(); const int horizon = context.getHorizon(); - // Extract control constraint auto control_constraint = context.getConstraint("ControlConstraint"); - // Terminal cost and its derivatives + // Terminal cost derivatives Eigen::VectorXd V_x = context.getObjective().getFinalCostGradient(context.X_.back()); Eigen::MatrixXd V_xx = context.getObjective().getFinalCostHessian(context.X_.back()); - // Pre-allocate matrices + // Pre-allocate 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::VectorXd Q_x(state_dim), 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); @@ -339,37 +106,30 @@ bool CLDDPSolver::backwardPass(CDDP &context) { double norm_Vx = V_x.lpNorm<1>(); double Qu_error = 0.0; - // Backward Riccati recursion for (int t = horizon - 1; t >= 0; --t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Get continuous dynamics Jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * context.getTimestep()); - // Convert to discrete time A = context.getTimestep() * Fx; A.diagonal().array() += 1.0; B = context.getTimestep() * Fu; - // Get cost and its derivatives auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().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; - // Apply regularization Q_uu_reg = Q_uu; Q_uu_reg.diagonal().array() += context.regularization_; - // Check positive definiteness Eigen::EigenSolver es(Q_uu_reg); if (es.eigenvalues().real().minCoeff() <= 0) { if (options.debug) { @@ -379,13 +139,11 @@ bool CLDDPSolver::backwardPass(CDDP &context) { return false; } - // Solve for control law if (control_constraint == nullptr) { const Eigen::MatrixXd H = Q_uu_reg.inverse(); k = -H * Q_u; K = -H * Q_ux; } else { - // Solve constrained QP const Eigen::VectorXd lb = control_constraint->rawLowerBound() - u; const Eigen::VectorXd ub = control_constraint->rawUpperBound() - u; const Eigen::VectorXd x0 = k_u_[t]; @@ -401,8 +159,6 @@ bool CLDDPSolver::backwardPass(CDDP &context) { } k = qp_result.x; - - // Compute feedback gain K = Eigen::MatrixXd::Zero(control_dim, state_dim); if (qp_result.free.sum() > 0) { std::vector free_idx; @@ -411,25 +167,20 @@ bool CLDDPSolver::backwardPass(CDDP &context) { free_idx.push_back(i); } } - 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.row(free_idx[i]); } - Eigen::MatrixXd K_free = -qp_result.Hfree.solve(Q_ux_free); - for (size_t i = 0; i < free_idx.size(); i++) { K.row(free_idx[i]) = K_free.row(i); } } } - // Store gains k_u_[t] = k; K_u_[t] = K; - // Update value function Eigen::Vector2d dV_step; dV_step << Q_u.dot(k), 0.5 * k.dot(Q_uu * k); dV_ += dV_step; @@ -438,88 +189,27 @@ bool CLDDPSolver::backwardPass(CDDP &context) { 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 + V_xx = 0.5 * (V_xx + V_xx.transpose()); norm_Vx += V_x.lpNorm<1>(); - - // Update optimality gap Qu_error = std::max(Qu_error, Q_u.lpNorm()); } - // Normalize dual infeasibility double scaling_factor = options.termination_scaling_max_factor; scaling_factor = std::max(scaling_factor, norm_Vx / (horizon * state_dim)) / scaling_factor; context.inf_du_ = Qu_error / scaling_factor; - if (options.debug) { - std::cout << "Qu_error: " << Qu_error << std::endl; - std::cout << "dV: " << dV_.transpose() << std::endl; - } - return true; } -ForwardPassResult CLDDPSolver::performForwardPass(CDDP &context) { - const CDDPOptions &options = context.getOptions(); - ForwardPassResult best_result; - best_result.cost = std::numeric_limits::infinity(); - best_result.success = false; - - if (!options.enable_parallel) { - // Single-threaded execution with early termination - for (double alpha_pr : context.alphas_) { - ForwardPassResult result = forwardPass(context, alpha_pr); - - if (result.success && result.cost < best_result.cost) { - best_result = result; - if (result.success) { - break; // Early termination - } - } - } - } else { - // Multi-threaded execution - std::vector> futures; - futures.reserve(context.alphas_.size()); - - for (double alpha_pr : context.alphas_) { - futures.push_back( - std::async(std::launch::async, [this, &context, alpha_pr]() { - return forwardPass(context, alpha_pr); - })); - } - - for (auto &future : futures) { - try { - if (future.valid()) { - ForwardPassResult result = future.get(); - if (result.success && result.cost < best_result.cost) { - best_result = result; - } - } - } catch (const std::exception &e) { - if (options.verbose) { - std::cerr << "CLDDP: Forward pass thread failed: " << e.what() - << std::endl; - } - } - } - } - - return best_result; -} - ForwardPassResult CLDDPSolver::forwardPass(CDDP &context, double alpha_pr) { - const CDDPOptions &options = context.getOptions(); - ForwardPassResult result; result.success = false; result.cost = std::numeric_limits::infinity(); result.merit_function = std::numeric_limits::infinity(); result.alpha_pr = alpha_pr; - // Initialize trajectories result.state_trajectory = context.X_; result.control_trajectory = context.U_; result.state_trajectory[0] = context.getInitialState(); @@ -528,65 +218,56 @@ ForwardPassResult CLDDPSolver::forwardPass(CDDP &context, double alpha_pr) { auto control_constraint = context.getConstraint("ControlConstraint"); - // Forward simulation for (int t = 0; t < context.getHorizon(); ++t) { const Eigen::VectorXd &x = result.state_trajectory[t]; - const Eigen::VectorXd &u = result.control_trajectory[t]; const Eigen::VectorXd delta_x = x - context.X_[t]; - // Apply control update - result.control_trajectory[t] = u + alpha_pr * k_u_[t] + K_u_[t] * delta_x; + result.control_trajectory[t] = + result.control_trajectory[t] + alpha_pr * k_u_[t] + K_u_[t] * delta_x; - // Apply control constraints if (control_constraint != nullptr) { result.control_trajectory[t] = control_constraint->clamp(result.control_trajectory[t]); } - // Compute running cost J_new += context.getObjective().running_cost(x, result.control_trajectory[t], t); - // Propagate dynamics result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( x, result.control_trajectory[t], t * context.getTimestep()); } - // Add terminal cost J_new += context.getObjective().terminal_cost(result.state_trajectory.back()); - // Check improvement double dJ = context.cost_ - J_new; double expected = -alpha_pr * (dV_(0) + 0.5 * alpha_pr * dV_(1)); double reduction_ratio = expected > 0.0 ? dJ / expected : std::copysign(1.0, dJ); - // Use the Armijo constant from filter options for line search acceptance + const CDDPOptions &options = context.getOptions(); result.success = reduction_ratio > options.filter.armijo_constant; result.cost = J_new; - result.merit_function = J_new; // For CLDDP, merit function equals cost + result.merit_function = J_new; return result; } -void CLDDPSolver::computeCost(CDDP &context) { - context.cost_ = 0.0; +bool CLDDPSolver::checkConvergence(CDDP &context, double dJ, double /*dL*/, + int /*iter*/, std::string &reason) { + const CDDPOptions &options = context.getOptions(); - // Running costs - for (int t = 0; t < context.getHorizon(); ++t) { - context.cost_ += - context.getObjective().running_cost(context.X_[t], context.U_[t], t); + if (context.inf_du_ < options.tolerance) { + reason = "OptimalSolutionFound"; + return true; } - - // Terminal cost - context.cost_ += context.getObjective().terminal_cost(context.X_.back()); - context.merit_function_ = - context.cost_; // For CLDDP, merit function equals cost + if (dJ > 0.0 && dJ < options.acceptable_tolerance) { + reason = "AcceptableSolutionFound"; + return true; + } + return false; } -void CLDDPSolver::printIteration(int iter, double cost, double merit, - double inf_du, double regularization, - double alpha) const { +void CLDDPSolver::printIteration(int iter, const CDDP &context) const { if (iter == 0) { std::cout << std::setw(4) << "iter" << " " << std::setw(12) << "objective" << " " << std::setw(10) << "inf_du" << " " << std::setw(8) @@ -594,28 +275,12 @@ void CLDDPSolver::printIteration(int iter, double cost, double merit, } std::cout << std::setw(4) << iter << " " << std::setw(12) << std::scientific - << std::setprecision(4) << cost << " " << std::setw(10) - << std::scientific << std::setprecision(2) << inf_du << " " - << std::setw(8) << std::fixed << std::setprecision(1) - << std::log10(regularization) << " " << std::setw(8) << std::fixed - << std::setprecision(4) << alpha << std::endl; -} - -void CLDDPSolver::printSolutionSummary(const CDDPSolution &solution) const { - std::cout << "\n========================================\n"; - std::cout << " CLDDP Solution Summary\n"; - std::cout << "========================================\n"; - - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); - auto status = std::any_cast(solution.at("status_message")); - - std::cout << "Status: " << status << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; - std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; - std::cout << "========================================\n\n"; + << std::setprecision(4) << context.cost_ << " " << std::setw(10) + << std::scientific << std::setprecision(2) << context.inf_du_ + << " " << std::setw(8) << std::fixed << std::setprecision(1) + << std::log10(context.regularization_) << " " << std::setw(8) + << std::fixed << std::setprecision(4) << context.alpha_pr_ + << std::endl; } } // namespace cddp diff --git a/src/cddp_core/ipddp_solver.cpp b/src/cddp_core/ipddp_solver.cpp index 4f8dd5e0..0648aacd 100644 --- a/src/cddp_core/ipddp_solver.cpp +++ b/src/cddp_core/ipddp_solver.cpp @@ -48,14 +48,14 @@ namespace cddp workspace_.Q_uu_matrices.resize(horizon); workspace_.Q_x_vectors.resize(horizon); workspace_.Q_u_vectors.resize(horizon); - + // Allocate LDLT solver cache workspace_.ldlt_solvers.resize(horizon); workspace_.ldlt_valid.resize(horizon, false); - + // Allocate forward pass workspace workspace_.delta_x_vectors.resize(horizon + 1); - + for (int t = 0; t < horizon; ++t) { workspace_.A_matrices[t] = Eigen::MatrixXd::Zero(state_dim, state_dim); workspace_.B_matrices[t] = Eigen::MatrixXd::Zero(state_dim, control_dim); @@ -65,11 +65,11 @@ namespace cddp workspace_.Q_x_vectors[t] = Eigen::VectorXd::Zero(state_dim); workspace_.Q_u_vectors[t] = Eigen::VectorXd::Zero(control_dim); } - + for (int t = 0; t <= horizon; ++t) { workspace_.delta_x_vectors[t] = Eigen::VectorXd::Zero(state_dim); } - + // Allocate constraint workspace if needed if (!constraint_set.empty()) { int total_dual_dim = getTotalDualDim(context); @@ -81,7 +81,7 @@ namespace cddp workspace_.YSinv = Eigen::MatrixXd::Zero(total_dual_dim, total_dual_dim); workspace_.bigRHS = Eigen::MatrixXd::Zero(control_dim, 1 + state_dim); } - + workspace_.initialized = true; } @@ -114,9 +114,6 @@ namespace cddp valid_warm_start = false; } - // For constrained problems, we don't require pre-existing dual/slack - // variables They will be re-initialized properly during warm start - if (valid_warm_start) { if (options.verbose) @@ -140,42 +137,34 @@ namespace cddp } // Initialize gains and constraints - k_u_.assign(horizon, Eigen::VectorXd::Zero(control_dim)); - K_u_.assign(horizon, Eigen::MatrixXd::Zero(control_dim, state_dim)); - dV_ = Eigen::Vector2d::Zero(); + initializeGains(horizon, control_dim, state_dim); initializeConstraintStorage(context); // Set barrier parameter based on constraint evaluation if (constraint_set.empty()) { - mu_ = 1e-8; // Small value if no constraints + mu_ = 1e-8; } else { - // Evaluate constraints and set barrier parameter evaluateTrajectoryWarmStart(context); double max_constraint_violation = computeMaxConstraintViolation(context); if (max_constraint_violation <= options.tolerance) { - mu_ = options.tolerance * 0.01; // Feasible trajectory + mu_ = options.tolerance * 0.01; } else if (max_constraint_violation <= 0.1) { - mu_ = options.tolerance; // Slightly infeasible + mu_ = options.tolerance; } else { - mu_ = options.ipddp.barrier.mu_initial * 0.1; // Significantly infeasible + mu_ = options.ipddp.barrier.mu_initial * 0.1; } } - // Initialize regularization context.regularization_ = options.regularization.initial_value; - - // Initialize step norm context.step_norm_ = 0.0; - - // Initialize dual/slack variables initializeDualSlackVariablesWarmStart(context); resetFilter(context); return; @@ -190,7 +179,6 @@ namespace cddp if (!trajectory_provided) { - // Create interpolated initial trajectory context.X_.resize(horizon + 1); context.U_.resize(horizon); @@ -216,9 +204,7 @@ namespace cddp } // Initialize gains, constraints, and parameters - k_u_.assign(horizon, Eigen::VectorXd::Zero(control_dim)); - K_u_.assign(horizon, Eigen::MatrixXd::Zero(control_dim, state_dim)); - dV_ = Eigen::Vector2d::Zero(); + initializeGains(horizon, control_dim, state_dim); initializeConstraintStorage(context); // Set barrier parameter @@ -251,262 +237,129 @@ namespace cddp return total_dual_dim; } - CDDPSolution IPDDPSolver::solve(CDDP &context) - { - const CDDPOptions &options = context.getOptions(); + // === CDDPSolverBase hook implementations === - // Print solver header if requested - if (options.print_solver_header) { - context.printSolverInfo(); - } - - // Print solver options if requested - if (options.print_solver_options) { - context.printOptions(options); - } + void IPDDPSolver::preIterationSetup(CDDP &context) + { + // evaluateTrajectory and resetFilter are already called in initialize(). + // No additional pre-iteration setup needed for IPDDP. + } - // Prepare solution map - CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("Running"); - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; - - // Initialize history vectors only if requested - std::vector history_objective; - std::vector history_merit_function; - std::vector history_step_length_primal; - std::vector history_step_length_dual; - std::vector history_dual_infeasibility; - std::vector history_primal_infeasibility; - std::vector history_complementary_infeasibility; - std::vector history_barrier_mu; - - if (options.return_iteration_info) - { - const size_t expected_size = - static_cast(options.max_iterations + 1); - history_objective.reserve(expected_size); - history_merit_function.reserve(expected_size); - history_step_length_primal.reserve(expected_size); - history_step_length_dual.reserve(expected_size); - history_dual_infeasibility.reserve(expected_size); - history_primal_infeasibility.reserve(expected_size); - history_complementary_infeasibility.reserve(expected_size); - history_barrier_mu.reserve(expected_size); - - // Initial iteration values - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_step_length_primal.push_back(1.0); // Initial step length - history_step_length_dual.push_back(1.0); // Initial dual step length - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(context.inf_pr_); - history_complementary_infeasibility.push_back(context.inf_comp_); - history_barrier_mu.push_back(mu_); - } + void IPDDPSolver::applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) + { + // Call base to update X_, U_, cost_, merit_function_, alpha_pr_, alpha_du_ + CDDPSolverBase::applyForwardPassResult(context, result); + + // Update IP-specific variables + if (result.dual_trajectory) + Y_ = *result.dual_trajectory; + if (result.slack_trajectory) + S_ = *result.slack_trajectory; + if (result.constraint_eval_trajectory) + G_ = *result.constraint_eval_trajectory; + } - if (options.verbose) - { - printIteration(0, context.cost_, context.inf_pr_, context.inf_du_, - context.inf_comp_, mu_, context.step_norm_, - context.regularization_, context.alpha_du_, - context.alpha_pr_); - } + bool IPDDPSolver::checkConvergence(CDDP &context, double dJ, double dL, + int iter, std::string &termination_reason) + { + const CDDPOptions &options = context.getOptions(); - // Main solve loop - auto start_time = std::chrono::high_resolution_clock::now(); - int iter = 0; - bool converged = false; - std::string termination_reason = "MaxIterationsReached"; - double dJ = 0.0; + // Compute IPOPT-style scaling factors + double scaled_inf_du = computeScaledDualInfeasibility(context); + double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - while (iter < options.max_iterations) + if (termination_metric <= options.tolerance) { - ++iter; - - // Check CPU time limit - if (options.max_cpu_time > 0) - { - auto duration = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start_time); - if (duration.count() > options.max_cpu_time * 1000) - { - termination_reason = "MaxCpuTimeReached"; - if (options.verbose) - { - std::cerr << "IPDDP: Maximum CPU time reached" << std::endl; - } - break; - } - } - - // Backward pass with regularization - bool backward_pass_success = false; - while (!backward_pass_success) + termination_reason = "OptimalSolutionFound"; + if (options.verbose) { - backward_pass_success = backwardPass(context); - if (!backward_pass_success) - { - context.increaseRegularization(); - if (context.isRegularizationLimitReached()) - { - termination_reason = "RegularizationLimitReached_NotConverged"; - if (options.verbose) - { - std::cerr << "IPDDP: Regularization limit reached" << std::endl; - } - break; - } - } + std::cout << "IPDDP: Converged due to scaled optimality gap and constraint " + "violation (metric: " + << std::scientific << std::setprecision(2) + << termination_metric << ", scaled inf_du: " << scaled_inf_du << ")" << std::endl; } - if (!backward_pass_success) - break; - - // Forward pass - ForwardPassResult best_result = performForwardPass(context); + return true; + } - // Update trajectories if forward pass succeeded - if (best_result.success) - { - if (options.debug) - { - std::cout << "[IPDDP Forward] cost: " << std::scientific << std::setprecision(4) - << best_result.cost << " α: " << best_result.alpha_pr - << " cv: " << best_result.constraint_violation << std::endl; - } + if (std::abs(dJ) < options.acceptable_tolerance && iter > 10) + { + bool acceptable_infeasibility = (context.inf_pr_ < std::sqrt(options.acceptable_tolerance) && + context.inf_comp_ < std::sqrt(options.acceptable_tolerance)); - // Update trajectories and variables - context.X_ = best_result.state_trajectory; - context.U_ = best_result.control_trajectory; - if (best_result.dual_trajectory) - Y_ = *best_result.dual_trajectory; - if (best_result.slack_trajectory) - S_ = *best_result.slack_trajectory; - if (best_result.constraint_eval_trajectory) - G_ = *best_result.constraint_eval_trajectory; - - // Update costs and step lengths - dJ = context.cost_ - best_result.cost; - context.cost_ = best_result.cost; - context.merit_function_ = best_result.merit_function; - context.alpha_pr_ = best_result.alpha_pr; - - updateIterationHistory(options, context, history_objective, history_merit_function, - history_step_length_primal, history_step_length_dual, - history_dual_infeasibility, history_primal_infeasibility, - history_complementary_infeasibility, history_barrier_mu, - best_result.alpha_du); - - context.decreaseRegularization(); - } - else + if (acceptable_infeasibility) { - context.increaseRegularization(); - if (context.isRegularizationLimitReached()) + termination_reason = "AcceptableSolutionFound"; + if (options.verbose) { - termination_reason = "RegularizationLimitReached_NotConverged"; - converged = false; - if (options.verbose) - { - std::cerr << "IPDDP: Regularization limit reached" << std::endl; - } - break; + std::cout << "IPDDP: Converged due to small change in cost (dJ: " + << std::scientific << std::setprecision(2) << std::abs(dJ) + << ") with acceptable infeasibility" << std::endl; } + return true; } + } - // Check convergence - converged = checkConvergence(options, context, dJ, iter, termination_reason); - if (converged) - break; - - // Print iteration info + if (iter >= 1 && + context.step_norm_ < options.tolerance * 10.0 && + context.inf_pr_ < 1e-4) + { + termination_reason = "AcceptableSolutionFound"; if (options.verbose) { - printIteration(iter, context.cost_, context.inf_pr_, context.inf_du_, - context.inf_comp_, mu_, context.step_norm_, - context.regularization_, best_result.alpha_du, - context.alpha_pr_); + std::cout << "IPDDP: Converged based on small step norm and feasibility" + << std::endl; } - - // Update barrier parameters using the extracted method - updateBarrierParameters(context, best_result.success); + return true; } - // Compute final timing - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - end_time - start_time); - - // Populate final solution - solution["status_message"] = termination_reason; - solution["iterations_completed"] = iter; - solution["solve_time_ms"] = static_cast(duration.count()); - solution["final_objective"] = context.cost_; - solution["final_step_length"] = context.alpha_pr_; - - // Add trajectories - std::vector time_points; - time_points.reserve(static_cast(context.getHorizon() + 1)); - for (int t = 0; t <= context.getHorizon(); ++t) - { - time_points.push_back(t * context.getTimestep()); - } - solution["time_points"] = time_points; - solution["state_trajectory"] = context.X_; - solution["control_trajectory"] = context.U_; + return false; + } - // Add iteration history if requested - if (options.return_iteration_info) - { - solution["history_objective"] = history_objective; - solution["history_merit_function"] = history_merit_function; - solution["history_step_length_primal"] = history_step_length_primal; - solution["history_step_length_dual"] = history_step_length_dual; - solution["history_dual_infeasibility"] = history_dual_infeasibility; - solution["history_primal_infeasibility"] = history_primal_infeasibility; - solution["history_complementary_infeasibility"] = history_complementary_infeasibility; - solution["history_barrier_mu"] = history_barrier_mu; - } + void IPDDPSolver::postIterationUpdate(CDDP &context, bool forward_pass_success) + { + updateBarrierParameters(context, forward_pass_success); + } - // Add control gains - solution["control_feedback_gains_K"] = K_u_; + void IPDDPSolver::recordIterationHistory(const CDDP &context) + { + CDDPSolverBase::recordIterationHistory(context); + history_.barrier_mu.push_back(mu_); + } - // Final metrics - solution["final_regularization"] = context.regularization_; + void IPDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) + { solution["final_barrier_parameter_mu"] = mu_; solution["final_primal_infeasibility"] = context.inf_pr_; solution["final_dual_infeasibility"] = context.inf_du_; solution["final_complementary_infeasibility"] = context.inf_comp_; + } - if (options.verbose) - { - printSolutionSummary(solution); - } - - - return solution; + void IPDDPSolver::printIteration(int iter, const CDDP &context) const + { + printIterationLegacy(iter, context.cost_, context.inf_pr_, context.inf_du_, + context.inf_comp_, mu_, context.step_norm_, + context.regularization_, context.alpha_du_, + context.alpha_pr_); } + // === Private methods (unchanged from original) === + void IPDDPSolver::evaluateTrajectory(CDDP &context) { const int horizon = context.getHorizon(); - const CDDPOptions &options = context.getOptions(); double cost = 0.0; - // Set initial state context.X_[0] = context.getInitialState(); - // Rollout dynamics and calculate cost for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute stage cost cost += context.getObjective().running_cost(x, u, t); - // For each constraint, evaluate and store the constraint value const auto &constraint_set = context.getConstraintSet(); for (const auto &constraint_pair : constraint_set) { @@ -516,15 +369,11 @@ namespace cddp G_[constraint_name][t] = g_val; } - // Compute next state using dynamics context.X_[t + 1] = context.getSystem().getDiscreteDynamics( x, u, t * context.getTimestep()); } - // Add terminal cost cost += context.getObjective().terminal_cost(context.X_.back()); - - // Store the cost context.cost_ = cost; } @@ -533,10 +382,6 @@ namespace cddp const int horizon = context.getHorizon(); double cost = 0.0; - // For warm start, the trajectory (X_, U_) is already provided - // We just need to evaluate the cost and constraints - - // Initialize constraint storage first const auto &constraint_set = context.getConstraintSet(); for (const auto &constraint_pair : constraint_set) { @@ -544,16 +389,13 @@ namespace cddp G_[constraint_name].resize(horizon); } - // Rollout dynamics and calculate cost for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute stage cost cost += context.getObjective().running_cost(x, u, t); - // For each constraint, evaluate and store the constraint value for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -563,10 +405,7 @@ namespace cddp } } - // Add terminal cost cost += context.getObjective().terminal_cost(context.X_.back()); - - // Store the cost context.cost_ = cost; } @@ -576,7 +415,6 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // Check if we have existing dual/slack variables from previous solve bool has_existing_dual_slack = true; for (const auto &constraint_pair : constraint_set) { @@ -591,7 +429,6 @@ namespace cddp } } - // Initialize/resize gains storage for all constraints k_y_.clear(); K_y_.clear(); k_s_.clear(); @@ -602,7 +439,6 @@ namespace cddp const std::string &constraint_name = constraint_pair.first; int dual_dim = constraint_pair.second->getDualDim(); - // Ensure proper sizing if (!has_existing_dual_slack) { Y_[constraint_name].resize(horizon); @@ -616,8 +452,6 @@ namespace cddp for (int t = 0; t < horizon; ++t) { - // Use the already evaluated constraint values from - // evaluateTrajectoryWarmStart const Eigen::VectorXd &g_val = G_[constraint_name][t]; bool need_reinit = false; @@ -628,25 +462,20 @@ namespace cddp y_current = Y_[constraint_name][t]; s_current = S_[constraint_name][t]; - // Check if existing dual/slack variables are feasible and compatible if (y_current.size() != dual_dim || s_current.size() != dual_dim) { need_reinit = true; } else { - // Check feasibility conditions for (int i = 0; i < dual_dim; ++i) { - // Check positivity: y_i > 0 and s_i > 0 if (y_current(i) <= 1e-12 || s_current(i) <= 1e-12) { need_reinit = true; break; } - // Check if constraint is severely violated (slack should be - // reasonable) double required_slack = std::max(options.ipddp.slack_var_init_scale, -g_val(i)); if (s_current(i) < 0.1 * required_slack) @@ -664,18 +493,13 @@ namespace cddp if (need_reinit) { - // Use the same initialization as cold start for consistency Eigen::VectorXd s_init = Eigen::VectorXd::Zero(dual_dim); Eigen::VectorXd y_init = Eigen::VectorXd::Zero(dual_dim); for (int i = 0; i < dual_dim; ++i) { - // Initialize s_i = max(slack_scale, -g_i) to ensure s_i > 0 (same as - // cold start) s_init(i) = std::max(options.ipddp.slack_var_init_scale, -g_val(i)); - // Initialize y_i = mu / s_i to satisfy s_i * y_i = mu (same as cold - // start) if (s_init(i) < 1e-12) { y_init(i) = mu_ / 1e-12; @@ -684,7 +508,6 @@ namespace cddp { y_init(i) = mu_ / s_init(i); } - // Clamp dual variable (same as cold start) y_init(i) = std::max( options.ipddp.dual_var_init_scale * 0.01, std::min(y_init(i), options.ipddp.dual_var_init_scale * 100.0)); @@ -693,7 +516,6 @@ namespace cddp S_[constraint_name][t] = s_init; } - // Always initialize gains to zero k_y_[constraint_name][t] = Eigen::VectorXd::Zero(dual_dim); K_y_[constraint_name][t] = Eigen::MatrixXd::Zero(dual_dim, context.getStateDim()); @@ -717,7 +539,6 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // Initialize dual and slack variables for each constraint for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -733,7 +554,6 @@ namespace cddp for (int t = 0; t < horizon; ++t) { - // Evaluate constraint g(x,u) = evaluate(x,u) - getUpperBound() Eigen::VectorXd g_val = constraint_pair.second->evaluate(context.X_[t], context.U_[t]) - constraint_pair.second->getUpperBound(); @@ -744,10 +564,8 @@ namespace cddp for (int i = 0; i < dual_dim; ++i) { - // Initialize s_i = max(slack_scale, -g_i) to ensure s_i > 0 s_init(i) = std::max(options.ipddp.slack_var_init_scale, -g_val(i)); - // Initialize y_i = mu / s_i to satisfy s_i * y_i = mu if (s_init(i) < 1e-12) { y_init(i) = mu_ / 1e-12; @@ -756,7 +574,6 @@ namespace cddp { y_init(i) = mu_ / s_init(i); } - // Clamp dual variable y_init(i) = std::max( options.ipddp.dual_var_init_scale * 0.01, std::min(y_init(i), options.ipddp.dual_var_init_scale * 100.0)); @@ -764,7 +581,6 @@ namespace cddp Y_[constraint_name][t] = y_init; S_[constraint_name][t] = s_init; - // Initialize gains to zero k_y_[constraint_name][t] = Eigen::VectorXd::Zero(dual_dim); K_y_[constraint_name][t] = Eigen::MatrixXd::Zero(dual_dim, context.getStateDim()); @@ -774,18 +590,16 @@ namespace cddp } } - // Initialize cost using objective evaluation context.cost_ = context.getObjective().evaluate(context.X_, context.U_); } void IPDDPSolver::resetBarrierFilter(CDDP &context) { - // Evaluate merit function (cost + log-barrier terms) double merit_function = context.cost_; - double inf_pr = 0.0; // inf_pr: infinity norm (largest absolute residual) - double filter_constraint_violation = 0.0; // l1 norm for filter (sum of residuals) - double inf_du = 0.0; // dual infeasibility (computed separately in backward pass) - double inf_comp = 0.0; // complementary infeasibility + double inf_pr = 0.0; + double filter_constraint_violation = 0.0; + double inf_du = 0.0; + double inf_comp = 0.0; const auto &constraint_set = context.getConstraintSet(); @@ -800,19 +614,12 @@ namespace cddp const Eigen::VectorXd &g_vec = G_[constraint_name][t]; const Eigen::VectorXd &y_vec = Y_[constraint_name][t]; - // Add log-barrier term merit_function -= mu_ * s_vec.array().log().sum(); - // Compute primal residual vector Eigen::VectorXd primal_residual = g_vec + s_vec; - - // inf_pr: infinity norm (largest absolute residual) inf_pr = std::max(inf_pr, primal_residual.lpNorm()); - - // Filter constraint violation: l1 norm (sum of residuals) filter_constraint_violation += primal_residual.lpNorm<1>(); - // Compute complementary infeasibility: ||y .* s - mu||_inf Eigen::VectorXd complementary_residual = y_vec.cwiseProduct(s_vec).array() - mu_; inf_comp = std::max(inf_comp, complementary_residual.lpNorm()); } @@ -820,7 +627,6 @@ namespace cddp } else { - // No constraints: set infeasibility metrics to zero inf_pr = 0.0; filter_constraint_violation = 0.0; inf_du = 0.0; @@ -828,11 +634,9 @@ namespace cddp } context.merit_function_ = merit_function; - context.inf_pr_ = inf_pr; // Store infinity norm as inf_pr - // Note: inf_du_ (dual infeasibility/optimality gap) is computed in backward pass for constrained case + context.inf_pr_ = inf_pr; context.inf_comp_ = inf_comp; - // Reset filter with initial point using l1 norm for constraint violation filter_.clear(); filter_.push_back(FilterPoint(merit_function, filter_constraint_violation)); } @@ -846,32 +650,27 @@ namespace cddp const int state_dim = context.getStateDim(); const double timestep = context.getTimestep(); - // Resize storage F_x_.resize(horizon); F_u_.resize(horizon); F_xx_.resize(horizon); F_uu_.resize(horizon); F_ux_.resize(horizon); - // Use parallel computation for larger horizons const int MIN_HORIZON_FOR_PARALLEL = 50; const bool use_parallel = options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; if (!use_parallel) { - // Single-threaded computation for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = @@ -882,7 +681,6 @@ namespace cddp } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); @@ -891,7 +689,6 @@ namespace cddp } else { - // Parallel computation const int num_threads = std::min(options.num_threads, static_cast(std::thread::hardware_concurrency())); const int chunk_size = std::max(1, horizon / num_threads); @@ -910,18 +707,15 @@ namespace cddp futures.push_back(std::async(std::launch::async, [this, &context, &options, start_t, end_t, timestep]() { - // Process chunk of time steps for (int t = start_t; t < end_t; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = context.getSystem().getHessians(x, u, t * timestep); @@ -929,7 +723,6 @@ namespace cddp F_uu_[t] = std::get<1>(hessians); F_ux_[t] = std::get<2>(hessians); } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); @@ -937,7 +730,6 @@ namespace cddp } })); } - // Wait for all computations to complete for (auto &future : futures) { try @@ -966,20 +758,17 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // If no constraints, return early if (constraint_set.empty()) { return; } - // Initialize storage for each constraint only if not already allocated for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; if (G_x_.find(constraint_name) == G_x_.end() || G_x_[constraint_name].size() != horizon) { G_x_[constraint_name].resize(horizon); G_u_[constraint_name].resize(horizon); - // Pre-allocate matrices with correct dimensions int state_dim = context.getStateDim(); int control_dim = context.getControlDim(); int constraint_dim = constraint_pair.second->getDualDim(); @@ -990,15 +779,12 @@ namespace cddp } } - // Threshold for when parallelization is worth it - increased for better - // performance const int MIN_HORIZON_FOR_PARALLEL = 50; const bool use_parallel = options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; if (!use_parallel) { - // Single-threaded computation for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; @@ -1016,7 +802,6 @@ namespace cddp } else { - // Chunked parallel computation const int num_threads = std::min(options.num_threads, static_cast(std::thread::hardware_concurrency())); @@ -1038,7 +823,6 @@ namespace cddp std::async(std::launch::async, [this, &context, &constraint_set, start_t, end_t]() { - // Process a chunk of time steps for (int t = start_t; t < end_t; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; @@ -1053,7 +837,6 @@ namespace cddp } })); } - // Wait for all computations to complete for (auto &future : futures) { try @@ -1086,26 +869,21 @@ namespace cddp const auto &constraint_set = context.getConstraintSet(); const int total_dual_dim = getTotalDualDim(context); - // Pre-compute dynamics jacobians and hessians for all time steps precomputeDynamicsDerivatives(context); - - // Pre-compute constraint gradients for all time steps and constraints precomputeConstraintGradients(context); - // Terminal cost and its derivatives Eigen::VectorXd V_x = context.getObjective().getFinalCostGradient(context.X_.back()); Eigen::MatrixXd V_xx = context.getObjective().getFinalCostHessian(context.X_.back()); - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize + V_xx = 0.5 * (V_xx + V_xx.transpose()); dV_ = Eigen::Vector2d::Zero(); - double inf_du = 0.0; // dual infeasibility (optimality gap; Qu_err) - double inf_pr = 0.0; // primal infeasibility (constraint violation) - double inf_comp = 0.0; // complementary infeasibility + double inf_du = 0.0; + double inf_pr = 0.0; + double inf_comp = 0.0; double step_norm = 0.0; - // If no constraints, use standard DDP recursion if (constraint_set.empty()) { for (int t = horizon - 1; t >= 0; --t) @@ -1113,38 +891,32 @@ namespace cddp const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Use pre-computed dynamics Jacobians const Eigen::MatrixXd &Fx = F_x_[t]; const Eigen::MatrixXd &Fu = F_u_[t]; - - // Use pre-allocated workspace matrices + Eigen::MatrixXd &A = workspace_.A_matrices[t]; Eigen::MatrixXd &B = workspace_.B_matrices[t]; A.noalias() = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx; B.noalias() = timestep * Fu; - // Cost & derivatives auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().getRunningCostHessians(x, u, t); - // Q expansions from cost - use pre-allocated workspace Eigen::VectorXd &Q_x = workspace_.Q_x_vectors[t]; Eigen::VectorXd &Q_u = workspace_.Q_u_vectors[t]; Eigen::MatrixXd &Q_xx = workspace_.Q_xx_matrices[t]; Eigen::MatrixXd &Q_ux = workspace_.Q_ux_matrices[t]; Eigen::MatrixXd &Q_uu = workspace_.Q_uu_matrices[t]; - + Q_x.noalias() = l_x + A.transpose() * V_x; Q_u.noalias() = l_u + B.transpose() * V_x; Q_xx.noalias() = l_xx + A.transpose() * V_xx * A; Q_ux.noalias() = l_ux + B.transpose() * V_xx * A; Q_uu.noalias() = l_uu + B.transpose() * V_xx * B; - // Add state hessian term if not using iLQR if (!options.use_ilqr) { - // Use pre-computed hessians const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; @@ -1157,20 +929,17 @@ namespace cddp } } - // Apply standard DDP regularization - Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); // symmetrize NOTE: This is critical + Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); Q_uu.diagonal().array() += context.regularization_; - // Use cached LDLT solver or compute new factorization - bool need_recompute = !workspace_.ldlt_valid[t] || + bool need_recompute = !workspace_.ldlt_valid[t] || (workspace_.ldlt_valid[t] && workspace_.ldlt_solvers[t].matrixLDLT().rows() != control_dim); - + if (need_recompute) { workspace_.ldlt_solvers[t].compute(Q_uu); workspace_.ldlt_valid[t] = true; } - // If valid and correct size, reuse existing factorization without recomputing - + if (workspace_.ldlt_solvers[t].info() != Eigen::Success) { if (options.debug) @@ -1186,27 +955,23 @@ namespace cddp k_u_[t] = k_u; K_u_[t] = K_u; - // Update value function V_x = Q_x + K_u.transpose() * Q_u + Q_ux.transpose() * k_u + K_u.transpose() * Q_uu * k_u; V_xx = Q_xx + K_u.transpose() * Q_ux + Q_ux.transpose() * K_u + K_u.transpose() * Q_uu * K_u; - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize + V_xx = 0.5 * (V_xx + V_xx.transpose()); - // Accumulate cost improvement dV_[0] += k_u.dot(Q_u); dV_[1] += 0.5 * k_u.dot(Q_uu * k_u); - // Error tracking inf_du = std::max(inf_du, Q_u.lpNorm()); step_norm = std::max(step_norm, k_u.lpNorm()); } - // Update termination metrics context.inf_du_ = inf_du; context.step_norm_ = step_norm; - context.inf_pr_ = 0.0; // No constraints - context.inf_comp_ = 0.0; // No complementary constraints + context.inf_pr_ = 0.0; + context.inf_comp_ = 0.0; if (options.debug) { @@ -1217,20 +982,17 @@ namespace cddp } else { - // Constrained backward recursion for (int t = horizon - 1; t >= 0; --t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Use pre-computed dynamics Jacobians const Eigen::MatrixXd &Fx = F_x_[t]; const Eigen::MatrixXd &Fu = F_u_[t]; Eigen::MatrixXd A = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx; Eigen::MatrixXd B = timestep * Fu; - // Use pre-allocated workspace for constraint variables Eigen::VectorXd &y = workspace_.y_combined; Eigen::VectorXd &s = workspace_.s_combined; Eigen::VectorXd &g = workspace_.g_combined; @@ -1258,22 +1020,18 @@ namespace cddp offset += dual_dim; } - // Cost & derivatives auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().getRunningCostHessians(x, u, t); - // Q expansions from cost Eigen::VectorXd Q_x = l_x + Q_yx.transpose() * y + A.transpose() * V_x; Eigen::VectorXd Q_u = l_u + Q_yu.transpose() * y + B.transpose() * V_x; Eigen::MatrixXd Q_xx = l_xx + A.transpose() * V_xx * A; Eigen::MatrixXd Q_ux = l_ux + B.transpose() * V_xx * A; Eigen::MatrixXd Q_uu = l_uu + B.transpose() * V_xx * B; - // Add state hessian term if not using iLQR if (!options.use_ilqr) { - // Use pre-computed hessians const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; @@ -1286,26 +1044,20 @@ namespace cddp } } - // Optimize diagonal matrix operations Eigen::MatrixXd &YSinv = workspace_.YSinv; YSinv.setZero(); for (int i = 0; i < total_dual_dim; ++i) { YSinv(i, i) = y(i) / s(i); } - // Residuals - Eigen::VectorXd primal_residual = g + s; // primal infeasibility - Eigen::VectorXd complementary_residual = y.cwiseProduct(s).array() - mu_; // complementary infeasibility + Eigen::VectorXd primal_residual = g + s; + Eigen::VectorXd complementary_residual = y.cwiseProduct(s).array() - mu_; Eigen::VectorXd rhat = y.cwiseProduct(primal_residual) - complementary_residual; - // Apply standard DDP regularization Eigen::MatrixXd Q_uu_reg = Q_uu; - Q_uu_reg = 0.5 * (Q_uu_reg + Q_uu_reg.transpose()); // symmetrize - - // Add constraint contribution + Q_uu_reg = 0.5 * (Q_uu_reg + Q_uu_reg.transpose()); + Q_uu_reg.noalias() += Q_yu.transpose() * YSinv * Q_yu; - - // Apply standard DDP regularization Q_uu_reg.diagonal().array() += context.regularization_; Eigen::LDLT ldlt(Q_uu_reg); @@ -1318,20 +1070,16 @@ namespace cddp return false; } - // Use pre-allocated workspace Eigen::MatrixXd &bigRHS = workspace_.bigRHS; - // Compute S_inv * rhat efficiently Eigen::VectorXd S_inv_rhat(total_dual_dim); for (int i = 0; i < total_dual_dim; ++i) { S_inv_rhat(i) = rhat(i) / s(i); } bigRHS.col(0).noalias() = Q_u + Q_yu.transpose() * S_inv_rhat; - // Compute M = Q_ux + Q_yu.transpose() * YSinv * Q_yx efficiently bigRHS.rightCols(state_dim).noalias() = Q_ux + Q_yu.transpose() * YSinv * Q_yx; Eigen::MatrixXd kK = -ldlt.solve(bigRHS); - // Parse out feedforward and feedback gains Eigen::VectorXd k_u = kK.col(0); Eigen::MatrixXd K_u(control_dim, state_dim); for (int col = 0; col < state_dim; col++) @@ -1342,7 +1090,6 @@ namespace cddp k_u_[t] = k_u; K_u_[t] = K_u; - // Compute gains for constraints efficiently Eigen::VectorXd k_y(total_dual_dim); Eigen::VectorXd temp = Q_yu * k_u; for (int i = 0; i < total_dual_dim; ++i) { @@ -1366,35 +1113,30 @@ namespace cddp offset += dual_dim; } - // Update Q expansions efficiently Q_u.noalias() += Q_yu.transpose() * S_inv_rhat; Q_x.noalias() += Q_yx.transpose() * S_inv_rhat; Q_xx.noalias() += Q_yx.transpose() * YSinv * Q_yx; Q_ux.noalias() += Q_yx.transpose() * YSinv * Q_yu; Q_uu.noalias() += Q_yu.transpose() * YSinv * Q_yu; - // Update cost improvement dV_[0] += k_u.dot(Q_u); dV_[1] += 0.5 * k_u.dot(Q_uu * k_u); - // Update value function V_x = Q_x + K_u.transpose() * Q_u + Q_ux.transpose() * k_u + K_u.transpose() * Q_uu * k_u; V_xx = Q_xx + K_u.transpose() * Q_ux + Q_ux.transpose() * K_u + K_u.transpose() * Q_uu * K_u; - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize + V_xx = 0.5 * (V_xx + V_xx.transpose()); - // Error tracking inf_du = std::max(inf_du, Q_u.lpNorm()); inf_pr = std::max(inf_pr, primal_residual.lpNorm()); inf_comp = std::max(inf_comp, complementary_residual.lpNorm()); step_norm = std::max(step_norm, k_u.lpNorm()); } - // Update termination metrics (properly separated) - context.inf_pr_ = inf_pr; // Primal infeasibility (constraint violation) - context.inf_du_ = inf_du; // Dual infeasibility (optimality gap) - context.inf_comp_ = inf_comp; // Complementary infeasibility + context.inf_pr_ = inf_pr; + context.inf_du_ = inf_du; + context.inf_comp_ = inf_comp; context.step_norm_ = step_norm; if (options.debug) @@ -1407,82 +1149,6 @@ namespace cddp } } - ForwardPassResult IPDDPSolver::performForwardPass(CDDP &context) - { - const CDDPOptions &options = context.getOptions(); - ForwardPassResult best_result; - best_result.cost = std::numeric_limits::infinity(); - best_result.merit_function = std::numeric_limits::infinity(); - best_result.success = false; - - // Store current merit function value and directional derivative - double merit_0 = context.merit_function_; - double directional_derivative = dV_(0); // Expected improvement - - // Adaptive backtracking parameters - const int max_adaptive_iterations = 5; - const double min_alpha = 1e-8; - - if (!options.enable_parallel) - { - // Single-threaded execution with early termination - for (double alpha_pr : context.alphas_) - { - ForwardPassResult result = forwardPass(context, alpha_pr); - - if (result.success && - result.merit_function < best_result.merit_function) - { - best_result = result; - if (result.success) - { - break; // Early termination - } - } - } - } - else - { - // Multi-threaded execution (use standard alphas for now) - std::vector> futures; - futures.reserve(context.alphas_.size()); - - for (double alpha_pr : context.alphas_) - { - futures.push_back( - std::async(std::launch::async, [this, &context, alpha_pr]() - { return forwardPass(context, alpha_pr); })); - } - - for (auto &future : futures) - { - try - { - if (future.valid()) - { - ForwardPassResult result = future.get(); - if (result.success && - result.merit_function < best_result.merit_function) - { - best_result = result; - } - } - } - catch (const std::exception &e) - { - if (options.verbose) - { - std::cerr << "IPDDP: Forward pass thread failed: " << e.what() - << std::endl; - } - } - } - } - - - return best_result; - } - ForwardPassResult IPDDPSolver::forwardPass(CDDP &context, double alpha) { const CDDPOptions &options = context.getOptions(); @@ -1498,7 +1164,6 @@ namespace cddp const double tau = std::max(options.ipddp.barrier.min_fraction_to_boundary, 1.0 - mu_); - // Initialize trajectories result.state_trajectory = context.X_; result.control_trajectory = context.U_; result.state_trajectory[0] = context.getInitialState(); @@ -1511,7 +1176,6 @@ namespace cddp double merit_function_new = 0.0; double constraint_violation_new = 0.0; - // Handle unconstrained case if (constraint_set.empty()) { for (int t = 0; t < horizon; ++t) @@ -1521,12 +1185,10 @@ namespace cddp result.control_trajectory[t] = context.U_[t] + alpha * k_u_[t] + K_u_[t] * delta_x; - // Propagate dynamics result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( result.state_trajectory[t], result.control_trajectory[t], t * context.getTimestep()); - // Accumulate stage cost cost_new += context.getObjective().running_cost( result.state_trajectory[t], result.control_trajectory[t], t); } @@ -1542,20 +1204,17 @@ namespace cddp result.cost = cost_new; result.merit_function = cost_new; result.constraint_violation = 0.0; - result.alpha_du = 1.0; // No dual variables for unconstrained case + result.alpha_du = 1.0; return result; } - // Constrained forward pass double alpha_s = alpha; - // Step 1: Update slack variables and state/control with alpha_s bool s_trajectory_feasible = true; for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd delta_x = result.state_trajectory[t] - context.X_[t]; - // Update slack variables first for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -1582,11 +1241,9 @@ namespace cddp if (!s_trajectory_feasible) break; - // Update control result.control_trajectory[t] = context.U_[t] + alpha_s * k_u_[t] + K_u_[t] * delta_x; - // Propagate dynamics result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( result.state_trajectory[t], result.control_trajectory[t], t * context.getTimestep()); @@ -1594,10 +1251,9 @@ namespace cddp if (!s_trajectory_feasible) { - return result; // Failed slack update + return result; } - // Step 2: Separate line search for dual variables bool suitable_alpha_y_found = false; std::map> Y_trial; @@ -1643,17 +1299,16 @@ namespace cddp { suitable_alpha_y_found = true; Y_new = Y_trial; - result.alpha_du = alpha_y_candidate; // Store the dual step size + result.alpha_du = alpha_y_candidate; break; } } if (!suitable_alpha_y_found) { - return result; // Failed dual variable update + return result; } - // Cost computation and filter line-search for (int t = 0; t < horizon; ++t) { cost_new += context.getObjective().running_cost( @@ -1670,7 +1325,6 @@ namespace cddp const Eigen::VectorXd &s_vec = S_new[constraint_name][t]; merit_function_new -= mu_ * s_vec.array().log().sum(); - // Primal infeasibility: g + s Eigen::VectorXd primal_residual = G_new[constraint_name][t] + s_vec; constraint_violation_new += primal_residual.lpNorm<1>(); } @@ -1680,13 +1334,11 @@ namespace cddp context.getObjective().terminal_cost(result.state_trajectory.back()); merit_function_new += cost_new; - // Filter acceptance logic bool filter_acceptance = false; double expected_improvement = alpha * dV_(0); double constraint_violation_old = filter_.empty() ? 0.0 : filter_.back().constraint_violation; double merit_function_old = context.merit_function_; - // Filter logic if (constraint_violation_new > options.filter.max_violation_threshold) { if (constraint_violation_new < (1 - options.filter.violation_acceptance_threshold) * constraint_violation_old) @@ -1732,7 +1384,7 @@ namespace cddp return result; } - void IPDDPSolver::printIteration(int iter, double objective, double inf_pr, + void IPDDPSolver::printIterationLegacy(int iter, double objective, double inf_pr, double inf_du, double inf_comp, double mu, double step_norm, double regularization, double alpha_du, double alpha_pr) const @@ -1748,26 +1400,20 @@ namespace cddp << "alpha_pr" << std::endl; } - // Format numbers with appropriate precision std::cout << std::setw(4) << iter << " "; - // Objective value std::cout << std::setw(12) << std::scientific << std::setprecision(6) << objective << " "; - // Primal infeasibility (constraint violation) std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_pr << " "; - // Dual infeasibility (optimality gap) std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_du << " "; - // Complementary infeasibility std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_comp << " "; - // Log of barrier parameter if (mu > 0.0) { std::cout << std::setw(7) << std::fixed << std::setprecision(1) @@ -1778,11 +1424,9 @@ namespace cddp std::cout << std::setw(7) << "-inf" << " "; } - // Step norm std::cout << std::setw(9) << std::scientific << std::setprecision(2) << step_norm << " "; - // Log of regularization if (regularization > 0.0) { std::cout << std::setw(7) << std::fixed << std::setprecision(1) @@ -1793,11 +1437,9 @@ namespace cddp std::cout << std::setw(7) << "-" << " "; } - // Dual step length std::cout << std::setw(9) << std::fixed << std::setprecision(6) << alpha_du << " "; - // Primal step length std::cout << std::setw(9) << std::fixed << std::setprecision(6) << alpha_pr; std::cout << std::endl; @@ -1832,33 +1474,28 @@ namespace cddp const auto &barrier_opts = options.ipddp.barrier; const auto &constraint_set = context.getConstraintSet(); - // Only update barrier parameters if we have constraints if (constraint_set.empty()) { - return; // No constraints case - no barrier update needed + return; } - // Select barrier update strategy switch (barrier_opts.strategy) { case BarrierStrategy::MONOTONIC: { - // Monotonic decrease: always reduce by fixed factor - mu_ = std::max(barrier_opts.mu_min_value, + mu_ = std::max(barrier_opts.mu_min_value, barrier_opts.mu_update_factor * mu_); resetFilter(context); break; } - + case BarrierStrategy::IPOPT: { - // IPOPT-style barrier update double scaled_inf_du = computeScaledDualInfeasibility(context); double error_k = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - - // IPOPT uses: μ_new = max(ε_tol/10, min(κ_μ * μ, μ^θ_μ)) - double kappa_epsilon = 10.0; // IPOPT default - + + double kappa_epsilon = 10.0; + if (error_k <= kappa_epsilon * mu_) { double new_mu_linear = barrier_opts.mu_update_factor * mu_; @@ -1869,52 +1506,43 @@ namespace cddp } break; } - + case BarrierStrategy::ADAPTIVE: default: { - // Current adaptive strategy (default) double scaled_inf_du = computeScaledDualInfeasibility(context); double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - // More aggressive barrier parameter update strategy double barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0); - + if (termination_metric <= barrier_update_threshold) { - // Adaptive barrier reduction strategy double reduction_factor = barrier_opts.mu_update_factor; if (mu_ > 1e-12) { double kkt_progress_ratio = termination_metric / mu_; - // Very aggressive reduction for good KKT satisfaction if (kkt_progress_ratio < 0.01) { reduction_factor = barrier_opts.mu_update_factor * 0.1; } - // Aggressive reduction if we're significantly satisfying KKT conditions else if (kkt_progress_ratio < 0.1) { reduction_factor = barrier_opts.mu_update_factor * 0.3; } - // Moderate reduction if we're moderately satisfying KKT conditions else if (kkt_progress_ratio < 0.5) { reduction_factor = barrier_opts.mu_update_factor * 0.6; } - // Standard reduction otherwise } - // Update barrier parameter with bounds double new_mu_linear = reduction_factor * mu_; double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power); mu_ = std::max(options.tolerance / 100.0, std::min(new_mu_linear, new_mu_superlinear)); - // Reset filter when barrier parameter changes resetFilter(context); } break; @@ -1922,101 +1550,11 @@ namespace cddp } } - void IPDDPSolver::updateIterationHistory( - const CDDPOptions &options, - const CDDP &context, - std::vector &history_objective, - std::vector &history_merit_function, - std::vector &history_step_length_primal, - std::vector &history_step_length_dual, - std::vector &history_dual_infeasibility, - std::vector &history_primal_infeasibility, - std::vector &history_complementary_infeasibility, - std::vector &history_barrier_mu, - double alpha_du) const - { - if (options.return_iteration_info) - { - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_step_length_primal.push_back(context.alpha_pr_); - history_step_length_dual.push_back(alpha_du); - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(context.inf_pr_); - history_complementary_infeasibility.push_back(context.inf_comp_); - history_barrier_mu.push_back(mu_); - } - } - - bool IPDDPSolver::checkConvergence( - const CDDPOptions &options, - const CDDP &context, - double dJ, - int iter, - std::string &termination_reason) const - { - // Compute IPOPT-style scaling factors - double scaled_inf_du = computeScaledDualInfeasibility(context); - double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - - if (termination_metric <= options.tolerance) - { - termination_reason = "OptimalSolutionFound"; - if (options.verbose) - { - std::cout << "IPDDP: Converged due to scaled optimality gap and constraint " - "violation (metric: " - << std::scientific << std::setprecision(2) - << termination_metric << ", scaled inf_du: " << scaled_inf_du << ")" << std::endl; - } - return true; - } - - // For acceptable tolerance, also check if we're making minimal progress over several iterations - if (std::abs(dJ) < options.acceptable_tolerance && iter > 10) - { - // Check if all infeasibility measures are reasonably small - bool acceptable_infeasibility = (context.inf_pr_ < std::sqrt(options.acceptable_tolerance) && - context.inf_comp_ < std::sqrt(options.acceptable_tolerance)); - - if (acceptable_infeasibility) - { - termination_reason = "AcceptableSolutionFound"; - if (options.verbose) - { - std::cout << "IPDDP: Converged due to small change in cost (dJ: " - << std::scientific << std::setprecision(2) << std::abs(dJ) - << ") with acceptable infeasibility" << std::endl; - } - return true; - } - } - - // Check step norm for early termination - if (iter >= 1 && - context.step_norm_ < options.tolerance * 10.0 && // Small step norm - context.inf_pr_ < 1e-4) // Reasonably feasible - { - termination_reason = "AcceptableSolutionFound"; - if (options.verbose) - { - std::cout << "IPDDP: Converged based on small step norm and feasibility" - << std::endl; - } - return true; - } - - return false; - } - - - void IPDDPSolver::initializeConstraintStorage(CDDP &context) { const auto &constraint_set = context.getConstraintSet(); const int horizon = context.getHorizon(); - // Clear and initialize constraint storage G_.clear(); G_x_.clear(); G_u_.clear(); @@ -2027,7 +1565,6 @@ namespace cddp k_s_.clear(); K_s_.clear(); - // Initialize storage for each constraint for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -2068,34 +1605,31 @@ namespace cddp const auto &constraint_set = context.getConstraintSet(); const int horizon = context.getHorizon(); const int control_dim = context.getControlDim(); - - // If no constraints, return the unscaled dual infeasibility + if (constraint_set.empty()) { return context.inf_du_; } - // IPOPT-style scaling: sd = max{smax, (||y||₁ + ||s||₁)/(m+n)}/smax - const double smax = 100.0; // Standard IPOPT value - - // Compute total L1 norms of dual and slack variables + const double smax = 100.0; + double y_norm_l1 = 0.0; double s_norm_l1 = 0.0; - int total_dual_dim = 0; // m: total number of constraints + int total_dual_dim = 0; for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; auto y_it = Y_.find(constraint_name); auto s_it = S_.find(constraint_name); - + if (y_it != Y_.end() && s_it != S_.end()) { for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &y_vec = y_it->second[t]; const Eigen::VectorXd &s_vec = s_it->second[t]; - + y_norm_l1 += y_vec.lpNorm<1>(); s_norm_l1 += s_vec.lpNorm<1>(); total_dual_dim += y_vec.size(); @@ -2103,17 +1637,13 @@ namespace cddp } } - // m = total_dual_dim (number of constraints) - // n = control_dim * horizon (number of control variables) int m = total_dual_dim; int n = control_dim * horizon; int m_plus_n = m + n; - // Compute scaling factor: sd = max{smax, (||y||₁ + ||s||₁)/(m+n)}/smax double scaling_numerator = (m_plus_n > 0) ? (y_norm_l1 + s_norm_l1) / static_cast(m_plus_n) : 0.0; double sd = std::max(smax, scaling_numerator) / smax; - - // Return scaled dual infeasibility + return context.inf_du_ / sd; } diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index ce43b439..b8f54b07 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -133,25 +133,14 @@ void LogDDPSolver::initialize(CDDP &context) { } } - // Resize linearized dynamics storage + // Resize dynamics storage F_.resize(horizon); - F_x_.resize(horizon); - F_u_.resize(horizon); - F_xx_.resize(horizon); - F_uu_.resize(horizon); - F_ux_.resize(horizon); - for (int t = 0; t < horizon; ++t) { F_[t] = Eigen::VectorXd::Zero(state_dim); } - k_u_.resize(horizon); - K_u_.resize(horizon); - - for (int t = 0; t < horizon; ++t) { - k_u_[t] = Eigen::VectorXd::Zero(control_dim); - K_u_[t] = Eigen::MatrixXd::Zero(control_dim, state_dim); - } + // Use base class helper for gains + initializeGains(horizon, control_dim, state_dim); G_.clear(); @@ -215,268 +204,109 @@ void LogDDPSolver::initialize(CDDP &context) { std::string LogDDPSolver::getSolverName() const { return "LogDDP"; } -CDDPSolution LogDDPSolver::solve(CDDP &context) { - const CDDPOptions &options = context.getOptions(); +// === Hook implementations === - // Print solver header if requested - if (options.print_solver_header) { - context.printSolverInfo(); - } - - // Print solver options if requested - if (options.print_solver_options) { - context.printOptions(options); - } - - // Prepare solution map with old-style structure for compatibility - CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("Running"); - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; - - // Initialize history vectors only if requested - std::vector history_objective; - std::vector history_lagrangian; - std::vector history_step_length_primal; - std::vector history_dual_infeasibility; - std::vector history_primal_infeasibility; - std::vector history_barrier_mu; - - if (options.return_iteration_info) { - const size_t expected_size = - static_cast(options.max_iterations + 1); - history_objective.reserve(expected_size); - history_lagrangian.reserve(expected_size); - history_step_length_primal.reserve(expected_size); - history_dual_infeasibility.reserve(expected_size); - history_primal_infeasibility.reserve(expected_size); - history_barrier_mu.reserve(expected_size); - } - - // Initialize trajectories and gaps - evaluateTrajectory(context); // context.cost_ is computed inside this function - if (options.return_iteration_info) { - history_objective.push_back(context.cost_); - } +void LogDDPSolver::preIterationSetup(CDDP &context) { + evaluateTrajectory(context); + resetFilter(context); +} - // Reset LogDDP filter - resetFilter(context); // L_ and constraint_violation_ are computed inside this - // function - if (options.return_iteration_info) { - history_lagrangian.push_back(context.merit_function_); - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(constraint_violation_); - history_barrier_mu.push_back(mu_); - } +void LogDDPSolver::applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) { + // Call base to update X_, U_, cost_, merit_function_, alpha_pr_, alpha_du_ + CDDPSolverBase::applyForwardPassResult(context, result); - if (options.verbose) { - printIteration(0, context.cost_, context.merit_function_, context.inf_du_, - context.regularization_, context.alpha_pr_, mu_, - constraint_violation_); + // Also update LogDDP-specific state + if (result.dynamics_trajectory) { + F_ = *result.dynamics_trajectory; } + constraint_violation_ = result.constraint_violation; +} - // Start timer - auto start_time = std::chrono::high_resolution_clock::now(); - int iter = 0; - bool converged = false; - std::string termination_reason = "MaxIterationsReached"; - double dJ = 0.0; - double dL = 0.0; - - // Main loop of LogDDP - while (iter < options.max_iterations) { - ++iter; - - // Check maximum CPU time - if (options.max_cpu_time > 0) { - auto current_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - current_time - start_time); - if (duration.count() > options.max_cpu_time * 1000) { - termination_reason = "MaxCpuTimeReached"; - if (options.verbose) { - std::cerr - << "LogDDP: 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 = backwardPass(context); - - if (!backward_pass_success) { - if (options.debug) { - std::cerr << "LogDDP: Backward pass failed" << std::endl; - } - - context.increaseRegularization(); - - if (context.isRegularizationLimitReached()) { - if (options.verbose) { - std::cerr << "LogDDP: Backward pass regularization limit reached!" - << std::endl; - } - converged = true; - termination_reason = "RegularizationLimitReached_Converged"; - break; - } - continue; - } - } - - if (converged) { - break; - } - - // 2. Forward pass - ForwardPassResult best_result = performForwardPass(context); - - bool forward_pass_success = best_result.success; - - // Update solution if a feasible forward pass was found - if (forward_pass_success) { - if (options.debug) { - std::cout << "[LogDDP: Forward pass] " << std::endl; - std::cout << " cost: " << best_result.cost << std::endl; - std::cout << " logcost: " << best_result.merit_function << std::endl; - std::cout << " alpha: " << best_result.alpha_pr << std::endl; - std::cout << " rf_err: " << best_result.constraint_violation - << std::endl; - } - - context.X_ = best_result.state_trajectory; - context.U_ = best_result.control_trajectory; - if (best_result.dynamics_trajectory) - F_ = *best_result.dynamics_trajectory; - - dJ = context.cost_ - best_result.cost; - context.cost_ = best_result.cost; - dL = context.merit_function_ - best_result.merit_function; - context.merit_function_ = best_result.merit_function; - context.alpha_pr_ = best_result.alpha_pr; - constraint_violation_ = best_result.constraint_violation; - - if (options.return_iteration_info) { - history_objective.push_back(context.cost_); - history_lagrangian.push_back(context.merit_function_); - history_step_length_primal.push_back(context.alpha_pr_); - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(constraint_violation_); - history_barrier_mu.push_back(mu_); - } - - context.decreaseRegularization(); - } else { - context.increaseRegularization(); +bool LogDDPSolver::checkConvergence(CDDP &context, double dJ, double dL, + int /*iter*/, std::string &reason) { + const CDDPOptions &options = context.getOptions(); - if (context.isRegularizationLimitReached()) { - if (options.debug) { - std::cerr << "LogDDP: Forward Pass regularization limit reached" - << std::endl; - } - converged = false; - termination_reason = "RegularizationLimitReached_NotConverged"; - break; - } + double termination_metric = std::max(context.inf_du_, context.inf_pr_); + if (termination_metric <= options.tolerance) { + if (options.debug) { + std::cout << "LogDDP: Converged due to optimality gap and constraint " + "violation." + << std::endl; } + reason = "OptimalSolutionFound"; + return true; + } - // Print iteration information - if (options.verbose) { - printIteration(iter, context.cost_, context.merit_function_, - context.inf_du_, context.regularization_, - context.alpha_pr_, mu_, constraint_violation_); + if (std::abs(dJ) < options.acceptable_tolerance && + std::abs(dL) < options.acceptable_tolerance) { + if (options.debug) { + std::cout + << "LogDDP: Converged due to small change in cost and Lagrangian." + << std::endl; } + reason = "AcceptableSolutionFound"; + return true; + } - // Check termination - double termination_metric = std::max(context.inf_du_, context.inf_pr_); - if (termination_metric <= options.tolerance) { - if (options.debug) { - std::cout << "LogDDP: Converged due to optimality gap and constraint " - "violation." - << std::endl; - } - converged = true; - termination_reason = "OptimalSolutionFound"; - break; - } + return false; +} - if (std::abs(dJ) < options.acceptable_tolerance && - std::abs(dL) < options.acceptable_tolerance) { - if (options.debug) { - std::cout - << "LogDDP: Converged due to small change in cost and Lagrangian." - << std::endl; - } - converged = true; - termination_reason = "AcceptableSolutionFound"; - break; - } +void LogDDPSolver::postIterationUpdate(CDDP &context, + bool forward_pass_success) { + const CDDPOptions &options = context.getOptions(); - // Barrier update logic - if (forward_pass_success) { - mu_ = std::max(options.log_barrier.barrier.mu_min_value, - mu_ * options.log_barrier.barrier.mu_update_factor); - } else { - mu_ = std::min(options.log_barrier.barrier.mu_initial, mu_ * 5.0); - } - relaxed_log_barrier_->setBarrierCoeff(mu_); - relaxed_log_barrier_->setRelaxationDelta(relaxation_delta_); - resetFilter(context); + // Barrier update logic + if (forward_pass_success) { + mu_ = std::max(options.log_barrier.barrier.mu_min_value, + mu_ * options.log_barrier.barrier.mu_update_factor); + } else { + mu_ = std::min(options.log_barrier.barrier.mu_initial, mu_ * 5.0); } + relaxed_log_barrier_->setBarrierCoeff(mu_); + relaxed_log_barrier_->setRelaxationDelta(relaxation_delta_); + resetFilter(context); +} - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - end_time - start_time); - - // Populate final solution - solution["status_message"] = termination_reason; - solution["iterations_completed"] = iter; - solution["solve_time_ms"] = static_cast(duration.count()); - solution["final_objective"] = context.cost_; - solution["final_step_length"] = context.alpha_pr_; - - // Add trajectories - std::vector time_points; - time_points.reserve(static_cast(context.getHorizon() + 1)); - for (int t = 0; t <= context.getHorizon(); ++t) { - time_points.push_back(t * context.getTimestep()); - } - solution["time_points"] = time_points; - solution["state_trajectory"] = context.X_; - solution["control_trajectory"] = context.U_; - - // Add iteration history if requested - if (options.return_iteration_info) { - solution["history_objective"] = history_objective; - solution["history_merit_function"] = history_lagrangian; - solution["history_step_length_primal"] = history_step_length_primal; - solution["history_dual_infeasibility"] = history_dual_infeasibility; - solution["history_primal_infeasibility"] = history_primal_infeasibility; - solution["history_barrier_mu"] = history_barrier_mu; - } +void LogDDPSolver::recordIterationHistory(const CDDP &context) { + // Call base to record common metrics + CDDPSolverBase::recordIterationHistory(context); - // Add control gains - solution["control_feedback_gains_K"] = K_u_; + // Add LogDDP-specific metrics + history_.barrier_mu.push_back(mu_); +} - // Final metrics - solution["final_regularization"] = context.regularization_; +void LogDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) { solution["final_barrier_parameter_mu"] = mu_; solution["final_primal_infeasibility"] = constraint_violation_; solution["final_dual_infeasibility"] = context.inf_du_; +} - if (options.verbose) { - printSolutionSummary(solution); +void LogDDPSolver::printIteration(int iter, const CDDP &context) const { + if (iter == 0) { + std::cout << std::setw(4) << "iter" << " " << std::setw(12) << "objective" + << " " << std::setw(12) << "lagrangian" << " " << std::setw(10) + << "opt_gap" << " " << std::setw(8) << "lg(rg)" << " " + << std::setw(8) << "alpha" << " " << std::setw(8) << "lg(mu)" + << " " << std::setw(10) << "cv_viol" << std::endl; } - - return solution; + std::cout << std::setw(4) << iter << " " << std::setw(12) << std::scientific + << std::setprecision(4) << context.cost_ << " " << std::setw(12) + << std::scientific << std::setprecision(4) << context.merit_function_ + << " " << std::setw(10) << std::scientific << std::setprecision(2) + << context.inf_du_ << " " << std::setw(8) << std::fixed + << std::setprecision(1) << std::log10(context.regularization_) + << " " << std::setw(8) << std::fixed << std::setprecision(4) + << context.alpha_pr_ << " " << std::setw(8) << std::fixed + << std::setprecision(1) << std::log10(mu_) << " " << std::setw(10) + << std::scientific << std::setprecision(2) << constraint_violation_ + << std::endl; } +// === Private helper methods === + void LogDDPSolver::evaluateTrajectory(CDDP &context) { const int horizon = context.getHorizon(); double cost = 0.0; @@ -535,11 +365,24 @@ void LogDDPSolver::resetFilter(CDDP &context) { context.inf_pr_ = constraint_violation_; } -void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { +// === Backward pass === + +bool LogDDPSolver::backwardPass(CDDP &context) { const CDDPOptions &options = context.getOptions(); - const int horizon = context.getHorizon(); const int state_dim = context.getStateDim(); + const int control_dim = context.getControlDim(); + const int horizon = context.getHorizon(); const double timestep = context.getTimestep(); + const auto &constraint_set = context.getConstraintSet(); + + // Pre-compute dynamics jacobians and hessians for all time steps + // Note: LogDDP needs continuous-time Jacobians for the backward pass + // because it handles defect terms (d = F[t] - X[t+1]) which require + // the A = dt*Fx + I, B = dt*Fu formulation. We compute these inline + // since the base precomputeDynamicsDerivatives stores discrete-time versions. + const int MIN_HORIZON_FOR_PARALLEL = 20; + const bool use_parallel = + options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; // Resize storage F_x_.resize(horizon); @@ -548,24 +391,16 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { F_uu_.resize(horizon); F_ux_.resize(horizon); - // Threshold for when parallelization is worth it - const int MIN_HORIZON_FOR_PARALLEL = 20; - const bool use_parallel = - options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; - if (!use_parallel) { - // Single-threaded computation - always efficient for small horizons for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = context.getSystem().getHessians(x, u, t * timestep); @@ -573,14 +408,12 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { F_uu_[t] = std::get<1>(hessians); F_ux_[t] = std::get<2>(hessians); } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); } } } else { - // Chunked parallel computation - much more efficient const int num_threads = std::min(options.num_threads, static_cast(std::thread::hardware_concurrency())); @@ -600,18 +433,15 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { futures.push_back( std::async(std::launch::async, [this, &context, &options, start_t, end_t, timestep]() { - // Process a chunk of time steps for (int t = start_t; t < end_t; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = context.getSystem().getHessians(x, u, t * timestep); @@ -619,7 +449,6 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { F_uu_[t] = std::get<1>(hessians); F_ux_[t] = std::get<2>(hessians); } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); @@ -628,7 +457,6 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { })); } - // Wait for all computations to complete for (auto &future : futures) { try { if (future.valid()) { @@ -651,18 +479,6 @@ void LogDDPSolver::precomputeDynamicsDerivatives(CDDP &context) { << (use_parallel ? "parallel" : "sequential") << " computation" << std::endl; } -} - -bool LogDDPSolver::backwardPass(CDDP &context) { - const CDDPOptions &options = context.getOptions(); - const int state_dim = context.getStateDim(); - const int control_dim = context.getControlDim(); - const int horizon = context.getHorizon(); - const double timestep = context.getTimestep(); - const auto &constraint_set = context.getConstraintSet(); - - // Pre-compute dynamics jacobians and hessians for all time steps - precomputeDynamicsDerivatives(context); // Terminal cost and derivatives (V_x, V_xx at t=N) Eigen::VectorXd V_x = @@ -681,7 +497,7 @@ bool LogDDPSolver::backwardPass(CDDP &context) { const Eigen::VectorXd &f = F_[t]; const Eigen::VectorXd &d = f - context.X_[t + 1]; // Defect - // Use pre-computed dynamics Jacobians + // Use pre-computed continuous-time dynamics Jacobians const Eigen::MatrixXd &Fx = F_x_[t]; const Eigen::MatrixXd &Fu = F_u_[t]; const Eigen::MatrixXd &A = @@ -701,7 +517,6 @@ bool LogDDPSolver::backwardPass(CDDP &context) { // Add state hessian term if not using iLQR if (!options.use_ilqr) { - // Use pre-computed hessians const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; @@ -788,55 +603,7 @@ bool LogDDPSolver::backwardPass(CDDP &context) { return true; } -ForwardPassResult LogDDPSolver::performForwardPass(CDDP &context) { - const CDDPOptions &options = context.getOptions(); - ForwardPassResult best_result; - best_result.cost = std::numeric_limits::infinity(); - best_result.merit_function = std::numeric_limits::infinity(); - best_result.success = false; - - if (!options.enable_parallel) { - // Single-threaded execution with early termination - for (double alpha_pr : context.alphas_) { - ForwardPassResult result = forwardPass(context, alpha_pr); - - if (result.success) { - best_result = result; - break; // Early termination on first success - } - } - } else { - // Multi-threaded execution - std::vector> futures; - futures.reserve(context.alphas_.size()); - - for (double alpha_pr : context.alphas_) { - futures.push_back( - std::async(std::launch::async, [this, &context, alpha_pr]() { - return forwardPass(context, alpha_pr); - })); - } - - for (auto &future : futures) { - try { - if (future.valid()) { - ForwardPassResult result = future.get(); - if (result.success && - result.merit_function < best_result.merit_function) { - best_result = result; - } - } - } catch (const std::exception &e) { - if (options.verbose) { - std::cerr << "LogDDP: Forward pass thread failed: " << e.what() - << std::endl; - } - } - } - } - - return best_result; -} +// === Forward pass === ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { const CDDPOptions &options = context.getOptions(); @@ -997,57 +764,4 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { return result; } -void LogDDPSolver::updateBarrierParameters(CDDP &context, - bool forward_pass_success, - double termination_metric) { - // This method is called from solve() with the specific barrier update logic - // from original The actual logic is implemented directly in solve() to match - // the original -} - -void LogDDPSolver::printIteration(int iter, double cost, double lagrangian, - double opt_gap, double regularization, - double alpha, double mu, - double constraint_violation) const { - if (iter == 0) { - std::cout << std::setw(4) << "iter" << " " << std::setw(12) << "objective" - << " " << std::setw(12) << "lagrangian" << " " << std::setw(10) - << "opt_gap" << " " << std::setw(8) << "lg(rg)" << " " - << std::setw(8) << "alpha" << " " << std::setw(8) << "lg(mu)" - << " " << std::setw(10) << "cv_viol" << std::endl; - } - - std::cout << std::setw(4) << iter << " " << std::setw(12) << std::scientific - << std::setprecision(4) << cost << " " << std::setw(12) - << std::scientific << std::setprecision(4) << lagrangian << " " - << std::setw(10) << std::scientific << std::setprecision(2) - << opt_gap << " " << std::setw(8) << std::fixed - << std::setprecision(1) << std::log10(regularization) << " " - << std::setw(8) << std::fixed << std::setprecision(4) << alpha - << " " << std::setw(8) << std::fixed << std::setprecision(1) - << std::log10(mu) << " " << std::setw(10) << std::scientific - << std::setprecision(2) << constraint_violation << std::endl; -} - -void LogDDPSolver::printSolutionSummary(const CDDPSolution &solution) const { - std::cout << "\n========================================\n"; - std::cout << " LogDDP Solution Summary\n"; - std::cout << "========================================\n"; - - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); - auto status = std::any_cast(solution.at("status_message")); - auto final_mu = - std::any_cast(solution.at("final_barrier_parameter_mu")); - - std::cout << "Status: " << status << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; - std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; - std::cout << "Final Barrier μ: " << std::setprecision(2) << std::scientific - << final_mu << "\n"; - std::cout << "========================================\n\n"; -} - } // namespace cddp diff --git a/src/cddp_core/msipddp_solver.cpp b/src/cddp_core/msipddp_solver.cpp index 19eafa32..54a4a465 100644 --- a/src/cddp_core/msipddp_solver.cpp +++ b/src/cddp_core/msipddp_solver.cpp @@ -40,7 +40,6 @@ namespace cddp // Initialize workspace if not already done if (!workspace_.initialized) { - // Allocate backward pass workspace workspace_.A_matrices.resize(horizon); workspace_.B_matrices.resize(horizon); workspace_.Q_xx_matrices.resize(horizon); @@ -48,17 +47,13 @@ namespace cddp workspace_.Q_uu_matrices.resize(horizon); workspace_.Q_x_vectors.resize(horizon); workspace_.Q_u_vectors.resize(horizon); - - // Allocate LDLT solver cache + workspace_.ldlt_solvers.resize(horizon); workspace_.ldlt_valid.resize(horizon, false); - - // Allocate forward pass workspace + workspace_.delta_x_vectors.resize(horizon + 1); - - // MSIPDDP-specific: defect vectors workspace_.d_vectors.resize(horizon); - + for (int t = 0; t < horizon; ++t) { workspace_.A_matrices[t] = Eigen::MatrixXd::Zero(state_dim, state_dim); workspace_.B_matrices[t] = Eigen::MatrixXd::Zero(state_dim, control_dim); @@ -69,12 +64,11 @@ namespace cddp workspace_.Q_u_vectors[t] = Eigen::VectorXd::Zero(control_dim); workspace_.d_vectors[t] = Eigen::VectorXd::Zero(state_dim); } - + for (int t = 0; t <= horizon; ++t) { workspace_.delta_x_vectors[t] = Eigen::VectorXd::Zero(state_dim); } - - // Allocate constraint workspace if needed + if (!constraint_set.empty()) { int total_dual_dim = getTotalDualDim(context); workspace_.y_combined = Eigen::VectorXd::Zero(total_dual_dim); @@ -85,7 +79,7 @@ namespace cddp workspace_.YSinv = Eigen::MatrixXd::Zero(total_dual_dim, total_dual_dim); workspace_.bigRHS = Eigen::MatrixXd::Zero(control_dim, 1 + state_dim); } - + workspace_.initialized = true; } @@ -118,9 +112,6 @@ namespace cddp valid_warm_start = false; } - // For constrained problems, we don't require pre-existing dual/slack - // variables They will be re-initialized properly during warm start - if (valid_warm_start) { if (options.verbose) @@ -137,16 +128,12 @@ namespace cddp } else { - // Warm start with provided trajectory (no existing solver state) if (options.verbose) { std::cout << "MSIPDDP: Warm start with provided trajectory" << std::endl; } - // Initialize gains and constraints - k_u_.assign(horizon, Eigen::VectorXd::Zero(control_dim)); - K_u_.assign(horizon, Eigen::MatrixXd::Zero(control_dim, state_dim)); - dV_ = Eigen::Vector2d::Zero(); + initializeGains(horizon, control_dim, state_dim); // Initialize MSIPDDP-specific costate variables and gains Lambda_.assign(horizon, Eigen::VectorXd::Zero(state_dim)); @@ -171,37 +158,30 @@ namespace cddp initializeConstraintStorage(context); - // Set barrier parameter based on constraint evaluation if (constraint_set.empty()) { - mu_ = 1e-8; // Small value if no constraints + mu_ = 1e-8; } else { - // Evaluate constraints and set barrier parameter evaluateTrajectoryWarmStart(context); double max_constraint_violation = computeMaxConstraintViolation(context); if (max_constraint_violation <= options.tolerance) { - mu_ = options.tolerance * 0.01; // Feasible trajectory + mu_ = options.tolerance * 0.01; } else if (max_constraint_violation <= 0.1) { - mu_ = options.tolerance; // Slightly infeasible + mu_ = options.tolerance; } else { - mu_ = options.msipddp.barrier.mu_initial * 0.1; // Significantly infeasible + mu_ = options.msipddp.barrier.mu_initial * 0.1; } } - // Initialize regularization context.regularization_ = options.regularization.initial_value; - - // Initialize step norm context.step_norm_ = 0.0; - - // Initialize dual/slack variables initializeDualSlackCostateVariablesWarmStart(context); resetFilter(context); return; @@ -216,7 +196,6 @@ namespace cddp if (!trajectory_provided) { - // Create interpolated initial trajectory context.X_.resize(horizon + 1); context.U_.resize(horizon); @@ -242,9 +221,7 @@ namespace cddp } // Initialize gains, constraints, and parameters - k_u_.assign(horizon, Eigen::VectorXd::Zero(control_dim)); - K_u_.assign(horizon, Eigen::MatrixXd::Zero(control_dim, state_dim)); - dV_ = Eigen::Vector2d::Zero(); + initializeGains(horizon, control_dim, state_dim); // Initialize MSIPDDP-specific costate variables and gains Lambda_.assign(horizon, Eigen::VectorXd::Zero(state_dim)); @@ -269,7 +246,6 @@ namespace cddp initializeConstraintStorage(context); - // Set barrier parameter if (constraint_set.empty()) { mu_ = 1e-8; @@ -299,279 +275,167 @@ namespace cddp return total_dual_dim; } - CDDPSolution MSIPDDPSolver::solve(CDDP &context) + // === CDDPSolverBase hook implementations === + + void MSIPDDPSolver::preIterationSetup(CDDP &context) { - const CDDPOptions &options = context.getOptions(); + // evaluateTrajectory and resetFilter are already called in initialize(). + // No additional pre-iteration setup needed for MSIPDDP. + } - // Print solver header if requested - if (options.print_solver_header) { - context.printSolverInfo(); - } + void MSIPDDPSolver::applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) + { + // Call base to update X_, U_, cost_, merit_function_, alpha_pr_, alpha_du_ + CDDPSolverBase::applyForwardPassResult(context, result); + + // Update IP-specific variables + if (result.dual_trajectory) + Y_ = *result.dual_trajectory; + if (result.slack_trajectory) + S_ = *result.slack_trajectory; + if (result.constraint_eval_trajectory) + G_ = *result.constraint_eval_trajectory; + + // Update MS-specific variables + if (result.dynamics_trajectory) + F_ = *result.dynamics_trajectory; + if (result.costate_trajectory) + Lambda_ = *result.costate_trajectory; + + // Update filter with accepted point + acceptFilterEntry(result.merit_function, result.constraint_violation); + } - // Print solver options if requested - if (options.print_solver_options) { - context.printOptions(options); - } + bool MSIPDDPSolver::checkConvergence(CDDP &context, double dJ, double dL, + int iter, std::string &termination_reason) + { + const CDDPOptions &options = context.getOptions(); - // Prepare solution map - CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("Running"); - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; - - // Initialize history vectors only if requested - std::vector history_objective; - std::vector history_merit_function; - std::vector history_step_length_primal; - std::vector history_step_length_dual; - std::vector history_dual_infeasibility; - std::vector history_primal_infeasibility; - std::vector history_complementary_infeasibility; - std::vector history_barrier_mu; - - if (options.return_iteration_info) - { - const size_t expected_size = - static_cast(options.max_iterations + 1); - history_objective.reserve(expected_size); - history_merit_function.reserve(expected_size); - history_step_length_primal.reserve(expected_size); - history_step_length_dual.reserve(expected_size); - history_dual_infeasibility.reserve(expected_size); - history_primal_infeasibility.reserve(expected_size); - history_complementary_infeasibility.reserve(expected_size); - history_barrier_mu.reserve(expected_size); - - // Initial iteration values - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_step_length_primal.push_back(1.0); // Initial step length - history_step_length_dual.push_back(1.0); // Initial dual step length - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(context.inf_pr_); - history_complementary_infeasibility.push_back(context.inf_comp_); - history_barrier_mu.push_back(mu_); - } + double scaled_inf_du = computeScaledDualInfeasibility(context); + double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - if (options.verbose) + if (termination_metric <= options.tolerance) { - printIteration(0, context.cost_, context.inf_pr_, context.inf_du_, - context.inf_comp_, mu_, context.step_norm_, - context.regularization_, context.alpha_du_, - context.alpha_pr_); + termination_reason = "OptimalSolutionFound"; + if (options.verbose) + { + std::cout << "MSIPDDP: Converged due to scaled optimality gap and constraint " + "violation (metric: " + << std::scientific << std::setprecision(2) + << termination_metric << ", scaled inf_du: " << scaled_inf_du << ")" << std::endl; + } + return true; } - // Main solve loop - auto start_time = std::chrono::high_resolution_clock::now(); - int iter = 0; - bool converged = false; - std::string termination_reason = "MaxIterationsReached"; - double dJ = 0.0; - - while (iter < options.max_iterations) + if (std::abs(dJ) < options.acceptable_tolerance && iter > 10) { - ++iter; + bool acceptable_infeasibility = (context.inf_pr_ < std::sqrt(options.acceptable_tolerance) && + context.inf_comp_ < std::sqrt(options.acceptable_tolerance)); - // Check CPU time limit - if (options.max_cpu_time > 0) + if (acceptable_infeasibility) { - auto duration = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start_time); - if (duration.count() > options.max_cpu_time * 1000) + termination_reason = "AcceptableSolutionFound"; + if (options.verbose) { - termination_reason = "MaxCpuTimeReached"; - if (options.verbose) - { - std::cerr << "MSIPDDP: Maximum CPU time reached" << std::endl; - } - break; + std::cout << "MSIPDDP: Converged due to small change in cost (dJ: " + << std::scientific << std::setprecision(2) << std::abs(dJ) + << ") with acceptable infeasibility" << std::endl; } + return true; } + } - // Backward pass with regularization - bool backward_pass_success = false; - while (!backward_pass_success) + if (iter >= 1 && + context.step_norm_ < options.tolerance * 10.0 && + context.inf_pr_ < 1e-4) + { + termination_reason = "AcceptableSolutionFound"; + if (options.verbose) { - backward_pass_success = backwardPass(context); - if (!backward_pass_success) - { - context.increaseRegularization(); - if (context.isRegularizationLimitReached()) - { - termination_reason = "RegularizationLimitReached_NotConverged"; - if (options.verbose) - { - std::cerr << "MSIPDDP: Regularization limit reached" << std::endl; - } - break; - } - } + std::cout << "MSIPDDP: Converged based on small step norm and feasibility" + << std::endl; } - if (!backward_pass_success) - break; + return true; + } - // Forward pass - ForwardPassResult best_result = performForwardPass(context); + return false; + } - // Update trajectories if forward pass succeeded - if (best_result.success) - { - if (options.debug) - { - std::cout << "[MSIPDDP Forward] cost: " << std::scientific << std::setprecision(4) - << best_result.cost << " α: " << best_result.alpha_pr - << " cv: " << best_result.constraint_violation << std::endl; - } + void MSIPDDPSolver::postIterationUpdate(CDDP &context, bool forward_pass_success) + { + updateBarrierParameters(context, forward_pass_success); + } - // Update trajectories and variables - context.X_ = best_result.state_trajectory; - context.U_ = best_result.control_trajectory; - if (best_result.dual_trajectory) - Y_ = *best_result.dual_trajectory; - if (best_result.slack_trajectory) - S_ = *best_result.slack_trajectory; - if (best_result.constraint_eval_trajectory) - G_ = *best_result.constraint_eval_trajectory; - if (best_result.dynamics_trajectory) - F_ = *best_result.dynamics_trajectory; - if (best_result.costate_trajectory) - Lambda_ = *best_result.costate_trajectory; - - // Update costs and step lengths - dJ = context.cost_ - best_result.cost; - context.cost_ = best_result.cost; - context.merit_function_ = best_result.merit_function; - context.alpha_pr_ = best_result.alpha_pr; - - // Update filter with accepted point - acceptFilterEntry(best_result.merit_function, best_result.constraint_violation); - - updateIterationHistory(options, context, history_objective, history_merit_function, - history_step_length_primal, history_step_length_dual, - history_dual_infeasibility, history_primal_infeasibility, - history_complementary_infeasibility, history_barrier_mu, - best_result.alpha_du); - - context.decreaseRegularization(); - } - else - { - // Try filter restoration before increasing regularization - bool restoration_performed = checkAndPerformFilterRestoration(context); - - if (!restoration_performed) - { - context.increaseRegularization(); - if (context.isRegularizationLimitReached()) - { - termination_reason = "RegularizationLimitReached_NotConverged"; - converged = false; - if (options.verbose) - { - std::cerr << "MSIPDDP: Regularization limit reached" << std::endl; - } - break; - } - } - else if (options.debug) - { - std::cout << "MSIPDDP: Filter restoration performed, retrying forward pass" << std::endl; - } - } + bool MSIPDDPSolver::handleForwardPassFailure(CDDP &context, + std::string &termination_reason) + { + const CDDPOptions &options = context.getOptions(); - // Check convergence - converged = checkConvergence(options, context, dJ, iter, termination_reason); - if (converged) - break; + // Try filter restoration before increasing regularization + bool restoration_performed = checkAndPerformFilterRestoration(context); - // Print iteration info - if (options.verbose) + if (!restoration_performed) + { + context.increaseRegularization(); + if (context.isRegularizationLimitReached()) { - printIteration(iter, context.cost_, context.inf_pr_, context.inf_du_, - context.inf_comp_, mu_, context.step_norm_, - context.regularization_, best_result.alpha_du, - context.alpha_pr_); + termination_reason = "RegularizationLimitReached_NotConverged"; + if (options.verbose) + { + std::cerr << "MSIPDDP: Regularization limit reached" << std::endl; + } + return true; // break } - - // Update barrier parameters using the extracted method - updateBarrierParameters(context, best_result.success); } - - // Compute final timing - auto end_time = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - end_time - start_time); - - // Populate final solution - solution["status_message"] = termination_reason; - solution["iterations_completed"] = iter; - solution["solve_time_ms"] = static_cast(duration.count()); - solution["final_objective"] = context.cost_; - solution["final_step_length"] = context.alpha_pr_; - - // Add trajectories - std::vector time_points; - time_points.reserve(static_cast(context.getHorizon() + 1)); - for (int t = 0; t <= context.getHorizon(); ++t) + else if (options.debug) { - time_points.push_back(t * context.getTimestep()); + std::cout << "MSIPDDP: Filter restoration performed, retrying forward pass" << std::endl; } - solution["time_points"] = time_points; - solution["state_trajectory"] = context.X_; - solution["control_trajectory"] = context.U_; - // Add iteration history if requested - if (options.return_iteration_info) - { - solution["history_objective"] = history_objective; - solution["history_merit_function"] = history_merit_function; - solution["history_step_length_primal"] = history_step_length_primal; - solution["history_step_length_dual"] = history_step_length_dual; - solution["history_dual_infeasibility"] = history_dual_infeasibility; - solution["history_primal_infeasibility"] = history_primal_infeasibility; - solution["history_complementary_infeasibility"] = history_complementary_infeasibility; - solution["history_barrier_mu"] = history_barrier_mu; - } + return false; // continue + } - // Add control gains - solution["control_feedback_gains_K"] = K_u_; + void MSIPDDPSolver::recordIterationHistory(const CDDP &context) + { + CDDPSolverBase::recordIterationHistory(context); + history_.barrier_mu.push_back(mu_); + } - // Final metrics - solution["final_regularization"] = context.regularization_; + void MSIPDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) + { solution["final_barrier_parameter_mu"] = mu_; solution["final_primal_infeasibility"] = context.inf_pr_; solution["final_dual_infeasibility"] = context.inf_du_; solution["final_complementary_infeasibility"] = context.inf_comp_; + } - if (options.verbose) - { - printSolutionSummary(solution); - } - - - return solution; + void MSIPDDPSolver::printIteration(int iter, const CDDP &context) const + { + printIterationLegacy(iter, context.cost_, context.inf_pr_, context.inf_du_, + context.inf_comp_, mu_, context.step_norm_, + context.regularization_, context.alpha_du_, + context.alpha_pr_); } + // === Private methods === + void MSIPDDPSolver::evaluateTrajectory(CDDP &context) { const CDDPOptions &options = context.getOptions(); const int horizon = context.getHorizon(); double cost = 0.0; - // Set initial state context.X_[0] = context.getInitialState(); - // Rollout dynamics and calculate cost for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute stage cost cost += context.getObjective().running_cost(x, u, t); - // For each constraint, evaluate and store the constraint value const auto &constraint_set = context.getConstraintSet(); for (const auto &constraint_pair : constraint_set) { @@ -581,18 +445,11 @@ namespace cddp G_[constraint_name][t] = g_val; } - // Evaluate and store dynamics for multi-shooting F_[t] = context.getSystem().getDiscreteDynamics(x, u, t * context.getTimestep()); - - // Compute next state using dynamics - // For initial trajectory evaluation, we always propagate the dynamics context.X_[t + 1] = F_[t]; } - // Add terminal cost cost += context.getObjective().terminal_cost(context.X_.back()); - - // Store the cost context.cost_ = cost; } @@ -602,10 +459,6 @@ namespace cddp const int horizon = context.getHorizon(); double cost = 0.0; - // For warm start, the trajectory (X_, U_) is already provided - // We just need to evaluate the cost and constraints - - // Initialize constraint storage first const auto &constraint_set = context.getConstraintSet(); for (const auto &constraint_pair : constraint_set) { @@ -613,16 +466,13 @@ namespace cddp G_[constraint_name].resize(horizon); } - // Rollout dynamics and calculate cost for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute stage cost cost += context.getObjective().running_cost(x, u, t); - // For each constraint, evaluate and store the constraint value for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -631,7 +481,6 @@ namespace cddp G_[constraint_name][t] = g_val; } - // Evaluate dynamics for multi-shooting F_[t] = context.getSystem().getDiscreteDynamics(x, u, t * context.getTimestep()); if (options.msipddp.use_controlled_rollout) @@ -640,10 +489,7 @@ namespace cddp } } - // Add terminal cost cost += context.getObjective().terminal_cost(context.X_.back()); - - // Store the cost context.cost_ = cost; } @@ -653,7 +499,6 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // Check if we have existing dual/slack variables from previous solve bool has_existing_dual_slack = true; for (const auto &constraint_pair : constraint_set) { @@ -668,7 +513,6 @@ namespace cddp } } - // Initialize/resize gains storage for all constraints k_y_.clear(); K_y_.clear(); k_s_.clear(); @@ -679,7 +523,6 @@ namespace cddp const std::string &constraint_name = constraint_pair.first; int dual_dim = constraint_pair.second->getDualDim(); - // Ensure proper sizing if (!has_existing_dual_slack) { Y_[constraint_name].resize(horizon); @@ -693,8 +536,6 @@ namespace cddp for (int t = 0; t < horizon; ++t) { - // Use the already evaluated constraint values from - // evaluateTrajectoryWarmStart const Eigen::VectorXd &g_val = G_[constraint_name][t]; bool need_reinit = false; @@ -705,25 +546,20 @@ namespace cddp y_current = Y_[constraint_name][t]; s_current = S_[constraint_name][t]; - // Check if existing dual/slack variables are feasible and compatible if (y_current.size() != dual_dim || s_current.size() != dual_dim) { need_reinit = true; } else { - // Check feasibility conditions for (int i = 0; i < dual_dim; ++i) { - // Check positivity: y_i > 0 and s_i > 0 if (y_current(i) <= 1e-12 || s_current(i) <= 1e-12) { need_reinit = true; break; } - // Check if constraint is severely violated (slack should be - // reasonable) double required_slack = std::max(options.msipddp.slack_var_init_scale, -g_val(i)); if (s_current(i) < 0.1 * required_slack) @@ -741,18 +577,13 @@ namespace cddp if (need_reinit) { - // Use the same initialization as cold start for consistency Eigen::VectorXd s_init = Eigen::VectorXd::Zero(dual_dim); Eigen::VectorXd y_init = Eigen::VectorXd::Zero(dual_dim); for (int i = 0; i < dual_dim; ++i) { - // Initialize s_i = max(slack_scale, -g_i) to ensure s_i > 0 (same as - // cold start) s_init(i) = std::max(options.msipddp.slack_var_init_scale, -g_val(i)); - // Initialize y_i = mu / s_i to satisfy s_i * y_i = mu (same as cold - // start) if (s_init(i) < 1e-12) { y_init(i) = mu_ / 1e-12; @@ -761,7 +592,6 @@ namespace cddp { y_init(i) = mu_ / s_init(i); } - // Clamp dual variable (same as cold start) y_init(i) = std::max( options.msipddp.dual_var_init_scale * 0.01, std::min(y_init(i), options.msipddp.dual_var_init_scale * 100.0)); @@ -770,7 +600,6 @@ namespace cddp S_[constraint_name][t] = s_init; } - // Always initialize gains to zero k_y_[constraint_name][t] = Eigen::VectorXd::Zero(dual_dim); K_y_[constraint_name][t] = Eigen::MatrixXd::Zero(dual_dim, context.getStateDim()); @@ -785,7 +614,6 @@ namespace cddp if (!has_existing_costate) { - // Initialize costate variables for first time for (int t = 0; t < horizon; ++t) { Lambda_[t] = options.msipddp.costate_var_init_scale * Eigen::VectorXd::Ones(context.getStateDim()); @@ -795,7 +623,6 @@ namespace cddp } else { - // Preserve existing costate variables, just initialize gains for (int t = 0; t < horizon; ++t) { k_lambda_[t] = Eigen::VectorXd::Zero(context.getStateDim()); @@ -818,7 +645,6 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // Initialize dual and slack variables for each constraint for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -834,7 +660,6 @@ namespace cddp for (int t = 0; t < horizon; ++t) { - // Evaluate constraint g(x,u) = evaluate(x,u) - getUpperBound() Eigen::VectorXd g_val = constraint_pair.second->evaluate(context.X_[t], context.U_[t]) - constraint_pair.second->getUpperBound(); @@ -845,10 +670,8 @@ namespace cddp for (int i = 0; i < dual_dim; ++i) { - // Initialize s_i = max(slack_scale, -g_i) to ensure s_i > 0 s_init(i) = std::max(options.msipddp.slack_var_init_scale, -g_val(i)); - // Initialize y_i = mu / s_i to satisfy s_i * y_i = mu if (s_init(i) < 1e-12) { y_init(i) = mu_ / 1e-12; @@ -857,7 +680,6 @@ namespace cddp { y_init(i) = mu_ / s_init(i); } - // Clamp dual variable y_init(i) = std::max( options.msipddp.dual_var_init_scale * 0.01, std::min(y_init(i), options.msipddp.dual_var_init_scale * 100.0)); @@ -865,7 +687,6 @@ namespace cddp Y_[constraint_name][t] = y_init; S_[constraint_name][t] = s_init; - // Initialize gains to zero k_y_[constraint_name][t] = Eigen::VectorXd::Zero(dual_dim); K_y_[constraint_name][t] = Eigen::MatrixXd::Zero(dual_dim, context.getStateDim()); @@ -878,27 +699,22 @@ namespace cddp // Initialize costate variables for MSIPDDP for (int t = 0; t < horizon; ++t) { - // Initialize costate variables with proper scaling Lambda_[t] = options.msipddp.costate_var_init_scale * Eigen::VectorXd::Ones(context.getStateDim()); - - // Initialize costate gains to zero k_lambda_[t] = Eigen::VectorXd::Zero(context.getStateDim()); K_lambda_[t] = Eigen::MatrixXd::Zero(context.getStateDim(), context.getStateDim()); } - // Initialize cost using objective evaluation context.cost_ = context.getObjective().evaluate(context.X_, context.U_); } void MSIPDDPSolver::resetBarrierFilter(CDDP &context) { - // Evaluate merit function (cost + log-barrier terms) double merit_function = context.cost_; - double inf_pr = 0.0; // inf_pr: infinity norm (largest absolute residual) - double filter_constraint_violation = 0.0; // l1 norm for filter (sum of residuals) - double inf_du = 0.0; // dual infeasibility (computed separately in backward pass) - double inf_comp = 0.0; // complementary infeasibility - double inf_defect = 0.0; // defect infeasibility for multi-shooting + double inf_pr = 0.0; + double filter_constraint_violation = 0.0; + double inf_du = 0.0; + double inf_comp = 0.0; + double inf_defect = 0.0; const auto &constraint_set = context.getConstraintSet(); @@ -913,24 +729,16 @@ namespace cddp const Eigen::VectorXd &g_vec = G_[constraint_name][t]; const Eigen::VectorXd &y_vec = Y_[constraint_name][t]; - // Add log-barrier term merit_function -= mu_ * s_vec.array().log().sum(); - // Compute primal residual vector Eigen::VectorXd primal_residual = g_vec + s_vec; - - // inf_pr: infinity norm (largest absolute residual) inf_pr = std::max(inf_pr, primal_residual.lpNorm()); - - // Filter constraint violation: l1 norm (sum of residuals) filter_constraint_violation += primal_residual.lpNorm<1>(); - // Compute complementary infeasibility: ||y .* s - mu||_inf Eigen::VectorXd complementary_residual = y_vec.cwiseProduct(s_vec).array() - mu_; inf_comp = std::max(inf_comp, complementary_residual.lpNorm()); } - // Add defect residual calculation if (t < static_cast(F_.size()) && (t + 1) < static_cast(context.X_.size())) { Eigen::VectorXd defect_residual = F_[t] - context.X_[t + 1]; @@ -941,20 +749,16 @@ namespace cddp } else { - // No constraints: set infeasibility metrics to zero inf_pr = 0.0; filter_constraint_violation = 0.0; inf_du = 0.0; inf_comp = 0.0; } - // Update primal infeasibility to include defect violations context.inf_pr_ = std::max(inf_pr, inf_defect); context.merit_function_ = merit_function; - // Note: inf_du_ (dual infeasibility/optimality gap) is computed in backward pass for constrained case context.inf_comp_ = inf_comp; - // Simple filter initialization with current point filter_.clear(); filter_.push_back(FilterPoint(merit_function, filter_constraint_violation)); } @@ -964,27 +768,24 @@ namespace cddp bool MSIPDDPSolver::acceptFilterEntry(double merit_function, double constraint_violation) { FilterPoint candidate(merit_function, constraint_violation); - - // Check if candidate is dominated by any existing filter point + for (const auto &filter_point : filter_) { if (filter_point.dominates(candidate)) { - return false; // Candidate is dominated, reject + return false; } } - - // Remove any filter points that are dominated by the candidate + filter_.erase( std::remove_if(filter_.begin(), filter_.end(), [&candidate](const FilterPoint &point) { return candidate.dominates(point); }), filter_.end()); - - // Add the candidate to the filter + filter_.push_back(candidate); - + return true; } @@ -992,13 +793,11 @@ namespace cddp const SolverSpecificFilterOptions &options, double expected_improvement) const { - // If filter is empty, any point is acceptable if (filter_.empty()) { return true; } - - // Quick domination check: reject if any filter point dominates candidate + FilterPoint candidate(merit_function, constraint_violation); for (const auto &filter_point : filter_) { @@ -1007,11 +806,10 @@ namespace cddp return false; } } - - // Find best filter point for comparison + double best_violation = std::numeric_limits::infinity(); double best_merit = std::numeric_limits::infinity(); - + for (const auto &filter_point : filter_) { if (filter_point.constraint_violation < best_violation) @@ -1020,34 +818,29 @@ namespace cddp best_merit = filter_point.merit_function; } } - - // Simple acceptance test: either improve constraint violation or merit function + bool violation_improvement = constraint_violation < best_violation * (1.0 - options.violation_acceptance_threshold); bool merit_improvement = merit_function < best_merit - options.merit_acceptance_threshold * constraint_violation; - - // For very small violations, use Armijo condition + if (constraint_violation < options.min_violation_for_armijo_check && expected_improvement < 0) { return merit_function < best_merit + options.armijo_constant * expected_improvement; } - - // Additional acceptance criterion: if we're very close to feasibility and making any improvement + if (constraint_violation < 1e-6 && merit_function <= best_merit * (1.0 + 1e-8)) { - return true; // Accept small improvements when nearly feasible + return true; } - + return violation_improvement || merit_improvement; } bool MSIPDDPSolver::checkAndPerformFilterRestoration(CDDP &context) { const CDDPOptions &options = context.getOptions(); - - // Simple restoration criteria: too many points or invalid values + bool needs_restoration = (filter_.size() > 5); - - // Check for numerical issues + if (!needs_restoration) { for (const auto &point : filter_) @@ -1060,25 +853,24 @@ namespace cddp } } } - + if (needs_restoration && !filter_.empty()) { if (options.debug) { std::cout << "MSIPDDP: Filter restoration: " << filter_.size() << " -> "; } - - // Keep only the best 2 points: best violation and best merit function + auto best_violation = *std::min_element(filter_.begin(), filter_.end(), [](const FilterPoint &a, const FilterPoint &b) { return a.constraint_violation < b.constraint_violation; }); - + auto best_merit = *std::min_element(filter_.begin(), filter_.end(), [](const FilterPoint &a, const FilterPoint &b) { return a.merit_function < b.merit_function; }); - + filter_.clear(); filter_.push_back(best_violation); if (std::abs(best_merit.constraint_violation - best_violation.constraint_violation) > 1e-12 || @@ -1086,15 +878,15 @@ namespace cddp { filter_.push_back(best_merit); } - + if (options.debug) { std::cout << filter_.size() << " points" << std::endl; } - + return true; } - + return false; } @@ -1105,32 +897,27 @@ namespace cddp const int state_dim = context.getStateDim(); const double timestep = context.getTimestep(); - // Resize storage F_x_.resize(horizon); F_u_.resize(horizon); F_xx_.resize(horizon); F_uu_.resize(horizon); F_ux_.resize(horizon); - // Use parallel computation for larger horizons const int MIN_HORIZON_FOR_PARALLEL = 50; const bool use_parallel = options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; if (!use_parallel) { - // Single-threaded computation for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = @@ -1141,7 +928,6 @@ namespace cddp } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); @@ -1150,7 +936,6 @@ namespace cddp } else { - // Parallel computation const int num_threads = std::min(options.num_threads, static_cast(std::thread::hardware_concurrency())); const int chunk_size = std::max(1, horizon / num_threads); @@ -1169,18 +954,15 @@ namespace cddp futures.push_back(std::async(std::launch::async, [this, &context, &options, start_t, end_t, timestep]() { - // Process chunk of time steps for (int t = start_t; t < end_t; ++t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // Compute jacobians const auto [Fx, Fu] = context.getSystem().getJacobians(x, u, t * timestep); F_x_[t] = Fx; F_u_[t] = Fu; - // Compute hessians if not using iLQR if (!options.use_ilqr) { const auto hessians = context.getSystem().getHessians(x, u, t * timestep); @@ -1188,7 +970,6 @@ namespace cddp F_uu_[t] = std::get<1>(hessians); F_ux_[t] = std::get<2>(hessians); } else { - // Initialize empty hessians for iLQR F_xx_[t] = std::vector(); F_uu_[t] = std::vector(); F_ux_[t] = std::vector(); @@ -1196,7 +977,6 @@ namespace cddp } })); } - // Wait for all computations to complete for (auto &future : futures) { try @@ -1225,27 +1005,23 @@ namespace cddp const int horizon = context.getHorizon(); const auto &constraint_set = context.getConstraintSet(); - // Clear and resize storage G_x_.clear(); G_u_.clear(); G_xx_.clear(); G_uu_.clear(); G_ux_.clear(); - // If no constraints, return early if (constraint_set.empty()) { return; } - // Initialize storage for each constraint only if not already allocated for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; if (G_x_.find(constraint_name) == G_x_.end() || G_x_[constraint_name].size() != horizon) { G_x_[constraint_name].resize(horizon); G_u_[constraint_name].resize(horizon); - // Pre-allocate matrices with correct dimensions int state_dim = context.getStateDim(); int control_dim = context.getControlDim(); int constraint_dim = constraint_pair.second->getDualDim(); @@ -1259,15 +1035,12 @@ namespace cddp G_ux_[constraint_name].resize(horizon); } - // Threshold for when parallelization is worth it - increased for better - // performance const int MIN_HORIZON_FOR_PARALLEL = 50; const bool use_parallel = options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; if (!use_parallel) { - // Single-threaded computation for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x = context.X_[t]; @@ -1281,7 +1054,6 @@ namespace cddp G_u_[constraint_name][t] = constraint_pair.second->getControlJacobian(x, u); - // Compute constraint hessians if not using iLQR if (!options.use_ilqr) { G_xx_[constraint_name][t] = constraint_pair.second->getStateHessian(x, u); @@ -1293,7 +1065,6 @@ namespace cddp } else { - // Chunked parallel computation const int num_threads = std::min(options.num_threads, static_cast(std::thread::hardware_concurrency())); @@ -1315,7 +1086,6 @@ namespace cddp std::async(std::launch::async, [this, &context, &constraint_set, start_t, end_t]() { - // Process a chunk of time steps const CDDPOptions &options = context.getOptions(); for (int t = start_t; t < end_t; ++t) { const Eigen::VectorXd &x = context.X_[t]; @@ -1327,8 +1097,7 @@ namespace cddp constraint_pair.second->getStateJacobian(x, u); G_u_[constraint_name][t] = constraint_pair.second->getControlJacobian(x, u); - - // Compute constraint hessians if not using iLQR + if (!options.use_ilqr) { G_xx_[constraint_name][t] = constraint_pair.second->getStateHessian(x, u); @@ -1339,7 +1108,6 @@ namespace cddp } })); } - // Wait for all computations to complete for (auto &future : futures) { try @@ -1372,27 +1140,22 @@ namespace cddp const auto &constraint_set = context.getConstraintSet(); const int total_dual_dim = getTotalDualDim(context); - // Pre-compute dynamics jacobians and hessians for all time steps precomputeDynamicsDerivatives(context); - - // Pre-compute constraint gradients for all time steps and constraints precomputeConstraintGradients(context); - // Terminal cost and its derivatives Eigen::VectorXd V_x = context.getObjective().getFinalCostGradient(context.X_.back()); Eigen::MatrixXd V_xx = context.getObjective().getFinalCostHessian(context.X_.back()); - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize + V_xx = 0.5 * (V_xx + V_xx.transpose()); dV_ = Eigen::Vector2d::Zero(); - double inf_du = 0.0; // dual infeasibility (optimality gap; Qu_err) - double inf_pr = 0.0; // primal infeasibility (constraint violation) - double inf_comp = 0.0; // complementary infeasibility - double inf_defect = 0.0; // defect norm for multi-shooting + double inf_du = 0.0; + double inf_pr = 0.0; + double inf_comp = 0.0; + double inf_defect = 0.0; double step_norm = 0.0; - // If no constraints, use standard DDP recursion if (constraint_set.empty()) { for (int t = horizon - 1; t >= 0; --t) @@ -1400,48 +1163,41 @@ namespace cddp const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // MSIPDDP: Access costate variables and compute defect const Eigen::VectorXd &lambda = Lambda_[t]; const Eigen::VectorXd &f = F_[t]; Eigen::VectorXd &d = workspace_.d_vectors[t]; d.setZero(); if ((t + 1) < static_cast(context.X_.size())) { - d = f - context.X_[t + 1]; // defect: dynamics mismatch + d = f - context.X_[t + 1]; } - // Use pre-computed dynamics Jacobians const Eigen::MatrixXd &Fx = F_x_[t]; const Eigen::MatrixXd &Fu = F_u_[t]; - - // Use pre-allocated workspace matrices + Eigen::MatrixXd &A = workspace_.A_matrices[t]; Eigen::MatrixXd &B = workspace_.B_matrices[t]; A.noalias() = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx; B.noalias() = timestep * Fu; - // Cost & derivatives auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().getRunningCostHessians(x, u, t); - // Q expansions from cost - use pre-allocated workspace Eigen::VectorXd &Q_x = workspace_.Q_x_vectors[t]; Eigen::VectorXd &Q_u = workspace_.Q_u_vectors[t]; Eigen::MatrixXd &Q_xx = workspace_.Q_xx_matrices[t]; Eigen::MatrixXd &Q_ux = workspace_.Q_ux_matrices[t]; Eigen::MatrixXd &Q_uu = workspace_.Q_uu_matrices[t]; - + Q_x.noalias() = l_x + A.transpose() * (V_x + V_xx * d); Q_u.noalias() = l_u + B.transpose() * (V_x + V_xx * d); Q_xx.noalias() = l_xx + A.transpose() * V_xx * A; Q_ux.noalias() = l_ux + B.transpose() * V_xx * A; Q_uu.noalias() = l_uu + B.transpose() * V_xx * B; - // Add state hessian term if not using iLQR if (!options.use_ilqr) { - // Use pre-computed hessians const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; @@ -1454,20 +1210,17 @@ namespace cddp } } - // Apply standard DDP regularization - Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); // symmetrize NOTE: This is critical + Q_uu = 0.5 * (Q_uu + Q_uu.transpose()); Q_uu.diagonal().array() += context.regularization_; - // Use cached LDLT solver or compute new factorization - bool need_recompute = !workspace_.ldlt_valid[t] || + bool need_recompute = !workspace_.ldlt_valid[t] || (workspace_.ldlt_valid[t] && workspace_.ldlt_solvers[t].matrixLDLT().rows() != control_dim); - + if (need_recompute) { workspace_.ldlt_solvers[t].compute(Q_uu); workspace_.ldlt_valid[t] = true; } - // If valid and correct size, reuse existing factorization without recomputing - + if (workspace_.ldlt_solvers[t].info() != Eigen::Success) { if (options.debug) @@ -1483,33 +1236,28 @@ namespace cddp k_u_[t] = k_u; K_u_[t] = K_u; - // MSIPDDP: Compute costate gains for multi-shooting k_lambda_[t] = -lambda + V_x + V_xx * d; K_lambda_[t] = V_xx; - K_lambda_[t] = 0.5 * (K_lambda_[t] + K_lambda_[t].transpose()); // Symmetrize + K_lambda_[t] = 0.5 * (K_lambda_[t] + K_lambda_[t].transpose()); - // Update value function V_x = Q_x + K_u.transpose() * Q_u + Q_ux.transpose() * k_u + K_u.transpose() * Q_uu * k_u; V_xx = Q_xx + K_u.transpose() * Q_ux + Q_ux.transpose() * K_u + K_u.transpose() * Q_uu * K_u; - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize + V_xx = 0.5 * (V_xx + V_xx.transpose()); - // Accumulate cost improvement dV_[0] += k_u.dot(Q_u); dV_[1] += 0.5 * k_u.dot(Q_uu * k_u); - // Error tracking inf_du = std::max(inf_du, Q_u.lpNorm()); step_norm = std::max(step_norm, k_u.lpNorm()); inf_defect = std::max(inf_defect, d.lpNorm()); } - // Update termination metrics context.inf_du_ = inf_du; context.step_norm_ = step_norm; - context.inf_pr_ = inf_defect; // Include defect violations as primal infeasibility - context.inf_comp_ = 0.0; // No complementary constraints + context.inf_pr_ = inf_defect; + context.inf_comp_ = 0.0; if (options.debug) { @@ -1520,33 +1268,28 @@ namespace cddp } else { - // Constrained backward recursion for (int t = horizon - 1; t >= 0; --t) { const Eigen::VectorXd &x = context.X_[t]; const Eigen::VectorXd &u = context.U_[t]; - // MSIPDDP: Access costate variables and compute defect const Eigen::VectorXd &lambda = Lambda_[t]; const Eigen::VectorXd &f = F_[t]; Eigen::VectorXd &d = workspace_.d_vectors[t]; d.setZero(); if ((t + 1) < static_cast(context.X_.size())) { - d = f - context.X_[t + 1]; // defect: dynamics mismatch + d = f - context.X_[t + 1]; } - // Use pre-computed dynamics Jacobians const Eigen::MatrixXd &Fx = F_x_[t]; const Eigen::MatrixXd &Fu = F_u_[t]; - - // Use pre-allocated workspace matrices + Eigen::MatrixXd &A = workspace_.A_matrices[t]; Eigen::MatrixXd &B = workspace_.B_matrices[t]; A.noalias() = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx; B.noalias() = timestep * Fu; - // Use pre-allocated workspace for constraint variables Eigen::VectorXd &y = workspace_.y_combined; Eigen::VectorXd &s = workspace_.s_combined; Eigen::VectorXd &g = workspace_.g_combined; @@ -1574,28 +1317,24 @@ namespace cddp offset += dual_dim; } - // Cost & derivatives auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().getRunningCostHessians(x, u, t); - // Q expansions from cost - use pre-allocated workspace Eigen::VectorXd &Q_x = workspace_.Q_x_vectors[t]; Eigen::VectorXd &Q_u = workspace_.Q_u_vectors[t]; Eigen::MatrixXd &Q_xx = workspace_.Q_xx_matrices[t]; Eigen::MatrixXd &Q_ux = workspace_.Q_ux_matrices[t]; Eigen::MatrixXd &Q_uu = workspace_.Q_uu_matrices[t]; - + Q_x.noalias() = l_x + Q_yx.transpose() * y + A.transpose() * (V_x + V_xx * d); Q_u.noalias() = l_u + Q_yu.transpose() * y + B.transpose() * (V_x + V_xx * d); Q_xx.noalias() = l_xx + A.transpose() * V_xx * A; Q_ux.noalias() = l_ux + B.transpose() * V_xx * A; Q_uu.noalias() = l_uu + B.transpose() * V_xx * B; - // Add state hessian term if not using iLQR if (!options.use_ilqr) { - // Use pre-computed hessians const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; @@ -1607,7 +1346,6 @@ namespace cddp Q_uu += timestep * lambda(i) * Fuu[i]; } - // Add constraint hessian terms int offset = 0; for (const auto &constraint_pair : constraint_set) { @@ -1628,26 +1366,20 @@ namespace cddp } } - // Optimize diagonal matrix operations Eigen::MatrixXd &YSinv = workspace_.YSinv; YSinv.setZero(); for (int i = 0; i < total_dual_dim; ++i) { YSinv(i, i) = y(i) / s(i); } - // Residuals - Eigen::VectorXd primal_residual = g + s; // primal infeasibility - Eigen::VectorXd complementary_residual = y.cwiseProduct(s).array() - mu_; // complementary infeasibility + Eigen::VectorXd primal_residual = g + s; + Eigen::VectorXd complementary_residual = y.cwiseProduct(s).array() - mu_; Eigen::VectorXd rhat = y.cwiseProduct(primal_residual) - complementary_residual; - // Apply standard DDP regularization Eigen::MatrixXd Q_uu_reg = Q_uu; - Q_uu_reg = 0.5 * (Q_uu_reg + Q_uu_reg.transpose()); // symmetrize - - // Add constraint contribution + Q_uu_reg = 0.5 * (Q_uu_reg + Q_uu_reg.transpose()); + Q_uu_reg.noalias() += Q_yu.transpose() * YSinv * Q_yu; - - // Apply standard DDP regularization Q_uu_reg.diagonal().array() += context.regularization_; Eigen::LDLT ldlt(Q_uu_reg); @@ -1660,20 +1392,16 @@ namespace cddp return false; } - // Use pre-allocated workspace Eigen::MatrixXd &bigRHS = workspace_.bigRHS; - // Compute S_inv * rhat efficiently Eigen::VectorXd S_inv_rhat(total_dual_dim); for (int i = 0; i < total_dual_dim; ++i) { S_inv_rhat(i) = rhat(i) / s(i); } bigRHS.col(0).noalias() = Q_u + Q_yu.transpose() * S_inv_rhat; - // Compute M = Q_ux + Q_yu.transpose() * YSinv * Q_yx efficiently bigRHS.rightCols(state_dim).noalias() = Q_ux + Q_yu.transpose() * YSinv * Q_yx; Eigen::MatrixXd kK = -ldlt.solve(bigRHS); - // Parse out feedforward and feedback gains Eigen::VectorXd k_u = kK.col(0); Eigen::MatrixXd K_u(control_dim, state_dim); for (int col = 0; col < state_dim; col++) @@ -1684,7 +1412,6 @@ namespace cddp k_u_[t] = k_u; K_u_[t] = K_u; - // Compute gains for constraints efficiently Eigen::VectorXd k_y(total_dual_dim); Eigen::VectorXd temp = Q_yu * k_u; for (int i = 0; i < total_dual_dim; ++i) { @@ -1708,30 +1435,25 @@ namespace cddp offset += dual_dim; } - // MSIPDDP: Compute costate gains for multi-shooting k_lambda_[t] = -lambda + V_x + V_xx * d; K_lambda_[t] = V_xx; - K_lambda_[t] = 0.5 * (K_lambda_[t] + K_lambda_[t].transpose()); // Symmetrize + K_lambda_[t] = 0.5 * (K_lambda_[t] + K_lambda_[t].transpose()); - // Update Q expansions efficiently Q_u.noalias() += Q_yu.transpose() * S_inv_rhat; Q_x.noalias() += Q_yx.transpose() * S_inv_rhat; Q_xx.noalias() += Q_yx.transpose() * YSinv * Q_yx; Q_ux.noalias() += Q_yx.transpose() * YSinv * Q_yu; Q_uu.noalias() += Q_yu.transpose() * YSinv * Q_yu; - // Update cost improvement dV_[0] += k_u.dot(Q_u); dV_[1] += 0.5 * k_u.dot(Q_uu * k_u); - // Update value function V_x = Q_x + K_u.transpose() * Q_u + Q_ux.transpose() * k_u + K_u.transpose() * Q_uu * k_u; V_xx = Q_xx + K_u.transpose() * Q_ux + Q_ux.transpose() * K_u + K_u.transpose() * Q_uu * K_u; - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize NOTE: This is critical + V_xx = 0.5 * (V_xx + V_xx.transpose()); - // Error tracking inf_du = std::max(inf_du, Q_u.lpNorm()); inf_pr = std::max(inf_pr, primal_residual.lpNorm()); inf_comp = std::max(inf_comp, complementary_residual.lpNorm()); @@ -1739,10 +1461,9 @@ namespace cddp step_norm = std::max(step_norm, k_u.lpNorm()); } - // Update termination metrics (properly separated) - context.inf_pr_ = std::max(inf_pr, inf_defect); // Primal infeasibility (constraint + defect violations) - context.inf_du_ = inf_du; // Dual infeasibility (optimality gap) - context.inf_comp_ = inf_comp; // Complementary infeasibility + context.inf_pr_ = std::max(inf_pr, inf_defect); + context.inf_du_ = inf_du; + context.inf_comp_ = inf_comp; context.step_norm_ = step_norm; if (options.debug) @@ -1755,73 +1476,6 @@ namespace cddp } } - ForwardPassResult MSIPDDPSolver::performForwardPass(CDDP &context) - { - const CDDPOptions &options = context.getOptions(); - ForwardPassResult best_result; - best_result.cost = std::numeric_limits::infinity(); - best_result.merit_function = std::numeric_limits::infinity(); - best_result.success = false; - - if (!options.enable_parallel) - { - // Single-threaded execution with early termination - for (double alpha_pr : context.alphas_) - { - ForwardPassResult result = forwardPass(context, alpha_pr); - - if (result.success && - result.merit_function < best_result.merit_function) - { - best_result = result; - if (result.success) - { - break; // Early termination - } - } - } - } - else - { - // Multi-threaded execution - std::vector> futures; - futures.reserve(context.alphas_.size()); - - for (double alpha_pr : context.alphas_) - { - futures.push_back( - std::async(std::launch::async, [this, &context, alpha_pr]() - { return forwardPass(context, alpha_pr); })); - } - - for (auto &future : futures) - { - try - { - if (future.valid()) - { - ForwardPassResult result = future.get(); - if (result.success && - result.merit_function < best_result.merit_function) - { - best_result = result; - } - } - } - catch (const std::exception &e) - { - if (options.verbose) - { - std::cerr << "MSIPDDP: Forward pass thread failed: " << e.what() - << std::endl; - } - } - } - } - - return best_result; - } - ForwardPassResult MSIPDDPSolver::forwardPass(CDDP &context, double alpha) { const CDDPOptions &options = context.getOptions(); @@ -1837,12 +1491,10 @@ namespace cddp const double tau = std::max(options.msipddp.barrier.min_fraction_to_boundary, 1.0 - mu_); - // Initialize trajectories result.state_trajectory = context.X_; result.control_trajectory = context.U_; result.state_trajectory[0] = context.getInitialState(); - // Initialize trajectories for forward pass std::vector F_new = F_; std::vector Lambda_new = Lambda_; std::map> Y_new = Y_; @@ -1853,48 +1505,39 @@ namespace cddp double merit_function_new = 0.0; double constraint_violation_new = 0.0; - // Handle unconstrained case if (constraint_set.empty()) { for (int t = 0; t < horizon; ++t) { - // Compute delta_x first before using it Eigen::VectorXd delta_x = result.state_trajectory[t] - context.X_[t]; workspace_.delta_x_vectors[t] = delta_x; result.control_trajectory[t] = context.U_[t] + alpha * k_u_[t] + K_u_[t] * delta_x; - // Update costate variables for multi-shooting if (t < static_cast(Lambda_new.size())) { Lambda_new[t] = Lambda_[t] + alpha * k_lambda_[t] + K_lambda_[t] * delta_x; } - // Determine if we're at a segment boundary for gap-closing bool is_segment_boundary = (ms_segment_length_ > 1) && ((t + 1) % ms_segment_length_ == 0) && (t + 1 < horizon); bool apply_gap_closing = is_segment_boundary; - // Evaluate dynamics at current point F_new[t] = context.getSystem().getDiscreteDynamics( result.state_trajectory[t], result.control_trajectory[t], t * context.getTimestep()); - // Apply multi-shooting strategy based on options if (apply_gap_closing) { - // Multi-shooting gap-closing at segment boundaries if (options.msipddp.rollout_type == "nonlinear") { - // Nonlinear rollout: Gap-closing with defect correction result.state_trajectory[t + 1] = context.X_[t + 1] + (F_new[t] - F_[t]) + alpha * (F_[t] - context.X_[t + 1]); } else if (options.msipddp.rollout_type == "hybrid") { - // Hybrid rollout: Linear approximation + defect correction const auto [Fx, Fu] = context.getSystem().getJacobians( context.X_[t], context.U_[t], t * context.getTimestep()); const double timestep = context.getTimestep(); @@ -1907,17 +1550,14 @@ namespace cddp } else { - // Default: standard dynamics propagation result.state_trajectory[t + 1] = F_new[t]; } } else { - // Normal propagation (not at segment boundary) result.state_trajectory[t + 1] = F_new[t]; } - // Accumulate stage cost cost_new += context.getObjective().running_cost( result.state_trajectory[t], result.control_trajectory[t], t); } @@ -1933,25 +1573,21 @@ namespace cddp result.cost = cost_new; result.merit_function = cost_new; result.constraint_violation = 0.0; - result.alpha_du = 1.0; // No dual variables for unconstrained case + result.alpha_du = 1.0; result.dynamics_trajectory = F_new; result.costate_trajectory = Lambda_new; return result; } - // Constrained forward pass double alpha_s = alpha; - // Step 1: Update slack variables and state/control with alpha_s bool s_trajectory_feasible = true; for (int t = 0; t < horizon; ++t) { - // Compute delta_x first before using it Eigen::VectorXd delta_x = result.state_trajectory[t] - context.X_[t]; workspace_.delta_x_vectors[t] = delta_x; - // Update slack variables first for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -1978,35 +1614,28 @@ namespace cddp if (!s_trajectory_feasible) break; - // Update control result.control_trajectory[t] = context.U_[t] + alpha_s * k_u_[t] + K_u_[t] * delta_x; - // Determine if we're at a segment boundary for gap-closing bool is_segment_boundary = (ms_segment_length_ > 1) && ((t + 1) % ms_segment_length_ == 0) && (t + 1 < horizon); bool apply_gap_closing = is_segment_boundary; - // Evaluate dynamics at current point F_new[t] = context.getSystem().getDiscreteDynamics( result.state_trajectory[t], result.control_trajectory[t], t * context.getTimestep()); - // Apply multi-shooting strategy based on options if (apply_gap_closing) { - // Multi-shooting gap-closing at segment boundaries if (options.msipddp.rollout_type == "nonlinear") { - // Nonlinear rollout: Gap-closing with defect correction result.state_trajectory[t + 1] = context.X_[t + 1] + (F_new[t] - F_[t]) + alpha_s * (F_[t] - context.X_[t + 1]); } else if (options.msipddp.rollout_type == "hybrid") { - // Hybrid rollout: Linear approximation + defect correction const auto [Fx, Fu] = context.getSystem().getJacobians( context.X_[t], context.U_[t], t * context.getTimestep()); const double timestep = context.getTimestep(); @@ -2019,23 +1648,20 @@ namespace cddp } else { - // Default: standard dynamics propagation result.state_trajectory[t + 1] = F_new[t]; } } else { - // Normal propagation (not at segment boundary) result.state_trajectory[t + 1] = F_new[t]; } } if (!s_trajectory_feasible) { - return result; // Failed slack update + return result; } - // Step 2: Separate line search for dual variables bool suitable_alpha_y_found = false; std::map> Y_trial; @@ -2073,7 +1699,6 @@ namespace cddp Y_trial[constraint_name][t] = y_new; } - // Update costate variables for multi-shooting if (t < static_cast(Lambda_new.size())) { Lambda_new[t] = Lambda_[t] + alpha_s * k_lambda_[t] + K_lambda_[t] * delta_x; @@ -2087,18 +1712,16 @@ namespace cddp { suitable_alpha_y_found = true; Y_new = Y_trial; - result.alpha_du = alpha_y_candidate; // Store the dual step size + result.alpha_du = alpha_y_candidate; break; } } if (!suitable_alpha_y_found) { - - return result; // Failed dual variable update + return result; } - // Cost computation and filter line-search for (int t = 0; t < horizon; ++t) { cost_new += context.getObjective().running_cost( @@ -2115,12 +1738,10 @@ namespace cddp const Eigen::VectorXd &s_vec = S_new[constraint_name][t]; merit_function_new -= mu_ * s_vec.array().log().sum(); - // Primal infeasibility: g + s Eigen::VectorXd primal_residual = G_new[constraint_name][t] + s_vec; constraint_violation_new += primal_residual.lpNorm<1>(); } - // Defect infeasibility: d = f - x_{k+1} Eigen::VectorXd defect_residual = F_new[t] - result.state_trajectory[t + 1]; constraint_violation_new += defect_residual.lpNorm<1>(); } @@ -2129,7 +1750,6 @@ namespace cddp context.getObjective().terminal_cost(result.state_trajectory.back()); merit_function_new += cost_new; - // Enhanced filter acceptance logic using new methods double expected_improvement = alpha * dV_(0); bool filter_acceptance = isFilterAcceptable(merit_function_new, constraint_violation_new, options.filter, expected_improvement); @@ -2150,7 +1770,7 @@ namespace cddp return result; } - void MSIPDDPSolver::printIteration(int iter, double objective, double inf_pr, + void MSIPDDPSolver::printIterationLegacy(int iter, double objective, double inf_pr, double inf_du, double inf_comp, double mu, double step_norm, double regularization, double alpha_du, double alpha_pr) const @@ -2166,26 +1786,20 @@ namespace cddp << "alpha_pr" << std::endl; } - // Format numbers with appropriate precision std::cout << std::setw(4) << iter << " "; - // Objective value std::cout << std::setw(12) << std::scientific << std::setprecision(6) << objective << " "; - // Primal infeasibility (constraint violation) std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_pr << " "; - // Dual infeasibility (optimality gap) std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_du << " "; - // Complementary infeasibility std::cout << std::setw(9) << std::scientific << std::setprecision(2) << inf_comp << " "; - // Log of barrier parameter if (mu > 0.0) { std::cout << std::setw(7) << std::fixed << std::setprecision(1) @@ -2196,11 +1810,9 @@ namespace cddp std::cout << std::setw(7) << "-inf" << " "; } - // Step norm std::cout << std::setw(9) << std::scientific << std::setprecision(2) << step_norm << " "; - // Log of regularization if (regularization > 0.0) { std::cout << std::setw(7) << std::fixed << std::setprecision(1) @@ -2211,11 +1823,9 @@ namespace cddp std::cout << std::setw(7) << "-" << " "; } - // Dual step length std::cout << std::setw(9) << std::fixed << std::setprecision(6) << alpha_du << " "; - // Primal step length std::cout << std::setw(9) << std::fixed << std::setprecision(6) << alpha_pr; std::cout << std::endl; @@ -2249,33 +1859,28 @@ namespace cddp const auto &barrier_opts = options.msipddp.barrier; const auto &constraint_set = context.getConstraintSet(); - // Only update barrier parameters if we have constraints if (constraint_set.empty()) { - return; // No constraints case - no barrier update needed + return; } - // Select barrier update strategy switch (barrier_opts.strategy) { case BarrierStrategy::MONOTONIC: { - // Monotonic decrease: always reduce by fixed factor - mu_ = std::max(barrier_opts.mu_min_value, + mu_ = std::max(barrier_opts.mu_min_value, barrier_opts.mu_update_factor * mu_); resetFilter(context); break; } - + case BarrierStrategy::IPOPT: { - // IPOPT-style barrier update double scaled_inf_du = computeScaledDualInfeasibility(context); double error_k = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - - // IPOPT uses: μ_new = max(ε_tol/10, min(κ_μ * μ, μ^θ_μ)) - double kappa_epsilon = 10.0; // IPOPT default - + + double kappa_epsilon = 10.0; + if (error_k <= kappa_epsilon * mu_) { double new_mu_linear = barrier_opts.mu_update_factor * mu_; @@ -2286,65 +1891,52 @@ namespace cddp } break; } - + case BarrierStrategy::ADAPTIVE: default: { - // Current adaptive strategy (default) double scaled_inf_du = computeScaledDualInfeasibility(context); double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - // Adaptive barrier parameter update strategy double barrier_update_threshold; - - // If mu is already small, use a more relaxed threshold + if (mu_ < 1e-5) { - // For small mu, update if we've made any reasonable progress barrier_update_threshold = std::max(termination_metric * 10.0, mu_ * 100.0); } else { - // Standard threshold for larger mu values barrier_update_threshold = std::max(barrier_opts.mu_update_factor * mu_, mu_ * 2.0); } - // Also consider updating if forward pass succeeded but with very small cost change NOTE: Hand tuning bool slow_progress = forward_pass_success && context.alpha_pr_ > 0 && - (termination_metric < 1e-3); // Only when reasonably feasible + (termination_metric < 1e-3); if (termination_metric <= barrier_update_threshold || slow_progress) { - // Adaptive barrier reduction strategy double reduction_factor = barrier_opts.mu_update_factor; if (mu_ > 1e-12) { double kkt_progress_ratio = termination_metric / mu_; - // Very aggressive reduction for good KKT satisfaction if (kkt_progress_ratio < 0.01) { reduction_factor = barrier_opts.mu_update_factor * 0.1; } - // Aggressive reduction if we're significantly satisfying KKT conditions else if (kkt_progress_ratio < 0.1) { reduction_factor = barrier_opts.mu_update_factor * 0.3; } - // Moderate reduction if we're moderately satisfying KKT conditions else if (kkt_progress_ratio < 0.5) { reduction_factor = barrier_opts.mu_update_factor * 0.6; } - // Standard reduction otherwise } - // Update barrier parameter with bounds double new_mu_linear = reduction_factor * mu_; double new_mu_superlinear = std::pow(mu_, barrier_opts.mu_update_power); - // Choose the more aggressive reduction when progress is slow if (slow_progress && mu_ > options.tolerance) { mu_ = std::min(new_mu_linear, new_mu_superlinear); @@ -2355,7 +1947,6 @@ namespace cddp std::min(new_mu_linear, new_mu_superlinear)); } - // Reset filter when barrier parameter changes resetFilter(context); } break; @@ -2363,99 +1954,11 @@ namespace cddp } } - void MSIPDDPSolver::updateIterationHistory( - const CDDPOptions &options, - const CDDP &context, - std::vector &history_objective, - std::vector &history_merit_function, - std::vector &history_step_length_primal, - std::vector &history_step_length_dual, - std::vector &history_dual_infeasibility, - std::vector &history_primal_infeasibility, - std::vector &history_complementary_infeasibility, - std::vector &history_barrier_mu, - double alpha_du) const - { - if (options.return_iteration_info) - { - history_objective.push_back(context.cost_); - history_merit_function.push_back(context.merit_function_); - history_step_length_primal.push_back(context.alpha_pr_); - history_step_length_dual.push_back(alpha_du); - history_dual_infeasibility.push_back(context.inf_du_); - history_primal_infeasibility.push_back(context.inf_pr_); - history_complementary_infeasibility.push_back(context.inf_comp_); - history_barrier_mu.push_back(mu_); - } - } - - bool MSIPDDPSolver::checkConvergence( - const CDDPOptions &options, - const CDDP &context, - double dJ, - int iter, - std::string &termination_reason) const - { - // Compute IPOPT-style scaling factors - double scaled_inf_du = computeScaledDualInfeasibility(context); - double termination_metric = std::max({scaled_inf_du, context.inf_pr_, context.inf_comp_}); - - if (termination_metric <= options.tolerance) - { - termination_reason = "OptimalSolutionFound"; - if (options.verbose) - { - std::cout << "MSIPDDP: Converged due to scaled optimality gap and constraint " - "violation (metric: " - << std::scientific << std::setprecision(2) - << termination_metric << ", scaled inf_du: " << scaled_inf_du << ")" << std::endl; - } - return true; - } - - // For acceptable tolerance, also check if we're making minimal progress over several iterations - if (std::abs(dJ) < options.acceptable_tolerance && iter > 10) - { - // Check if all infeasibility measures are reasonably small - bool acceptable_infeasibility = (context.inf_pr_ < std::sqrt(options.acceptable_tolerance) && - context.inf_comp_ < std::sqrt(options.acceptable_tolerance)); - - if (acceptable_infeasibility) - { - termination_reason = "AcceptableSolutionFound"; - if (options.verbose) - { - std::cout << "MSIPDDP: Converged due to small change in cost (dJ: " - << std::scientific << std::setprecision(2) << std::abs(dJ) - << ") with acceptable infeasibility" << std::endl; - } - return true; - } - } - - // Check step norm for early termination - if (iter >= 1 && - context.step_norm_ < options.tolerance * 10.0 && // Small step norm - context.inf_pr_ < 1e-4) // Reasonably feasible - { - termination_reason = "AcceptableSolutionFound"; - if (options.verbose) - { - std::cout << "MSIPDDP: Converged based on small step norm and feasibility" - << std::endl; - } - return true; - } - - return false; - } - void MSIPDDPSolver::initializeConstraintStorage(CDDP &context) { const auto &constraint_set = context.getConstraintSet(); const int horizon = context.getHorizon(); - // Clear and initialize constraint storage G_.clear(); G_x_.clear(); G_u_.clear(); @@ -2466,7 +1969,6 @@ namespace cddp k_s_.clear(); K_s_.clear(); - // Initialize storage for each constraint for (const auto &constraint_pair : constraint_set) { const std::string &constraint_name = constraint_pair.first; @@ -2508,19 +2010,16 @@ namespace cddp const int horizon = context.getHorizon(); const int control_dim = context.getControlDim(); - // If no constraints, return the unscaled dual infeasibility if (constraint_set.empty()) { return context.inf_du_; } - // IPOPT-style scaling: sd = max{smax, (||y||₁ + ||s||₁)/(m+n)}/smax - const double smax = 100.0; // Standard IPOPT value + const double smax = 100.0; - // Compute total L1 norms of dual and slack variables double y_norm_l1 = 0.0; double s_norm_l1 = 0.0; - int total_dual_dim = 0; // m: total number of constraints + int total_dual_dim = 0; for (const auto &constraint_pair : constraint_set) { @@ -2542,17 +2041,13 @@ namespace cddp } } - // m = total_dual_dim (number of constraints) - // n = control_dim * horizon (number of control variables) int m = total_dual_dim; int n = control_dim * horizon; int m_plus_n = m + n; - // Compute scaling factor: sd = max{smax, (||y||₁ + ||s||₁)/(m+n)}/smax double scaling_numerator = (m_plus_n > 0) ? (y_norm_l1 + s_norm_l1) / static_cast(m_plus_n) : 0.0; double sd = std::max(smax, scaling_numerator) / smax; - // Return scaled dual infeasibility return context.inf_du_ / sd; } diff --git a/src/dynamics_model/bicycle.cpp b/src/dynamics_model/bicycle.cpp index 2754c43f..0d08869d 100644 --- a/src/dynamics_model/bicycle.cpp +++ b/src/dynamics_model/bicycle.cpp @@ -131,10 +131,7 @@ std::vector Bicycle::getStateHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Extract state variables const double theta = state(STATE_THETA); // heading angle @@ -162,10 +159,7 @@ std::vector Bicycle::getControlHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // Extract state variables const double v = state(STATE_V); // velocity diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index 909a7e52..37b08a57 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -121,10 +121,7 @@ std::vector Car::getStateHessian( VectorXdual2nd control_dual = control.cast(); // Initialize Hessians - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { @@ -152,10 +149,7 @@ std::vector Car::getControlHessian( VectorXdual2nd control_dual = control.cast(); // Initialize Hessians - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { diff --git a/src/dynamics_model/dreyfus_rocket.cpp b/src/dynamics_model/dreyfus_rocket.cpp index b65c44e5..65d6656d 100644 --- a/src/dynamics_model/dreyfus_rocket.cpp +++ b/src/dynamics_model/dreyfus_rocket.cpp @@ -65,20 +65,14 @@ Eigen::MatrixXd DreyfusRocket::getControlJacobian( std::vector DreyfusRocket::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } std::vector DreyfusRocket::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // The only non-zero element is the second derivative of x_dot with respect to theta const double theta = control(CONTROL_THETA); diff --git a/src/dynamics_model/dubins_car.cpp b/src/dynamics_model/dubins_car.cpp index b045bbc4..3a807948 100644 --- a/src/dynamics_model/dubins_car.cpp +++ b/src/dynamics_model/dubins_car.cpp @@ -109,10 +109,7 @@ std::vector DubinsCar::getStateHessian( (void)control; // Not used for state Hessian // Initialize vector of matrices (one matrix per state dimension) - std::vector hessian(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessian[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessian = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Extract state components const double theta = state(STATE_THETA); @@ -136,10 +133,7 @@ std::vector DubinsCar::getControlHessian( (void)state; (void)control; - std::vector hessian(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessian = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessian; diff --git a/src/dynamics_model/forklift.cpp b/src/dynamics_model/forklift.cpp index 3c6d310f..9b32bf1d 100644 --- a/src/dynamics_model/forklift.cpp +++ b/src/dynamics_model/forklift.cpp @@ -111,10 +111,7 @@ std::vector Forklift::getStateHessian( VectorXdual2nd control_dual = control.cast(); // Initialize Hessians - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { @@ -138,10 +135,7 @@ std::vector Forklift::getControlHessian( VectorXdual2nd control_dual = control.cast(); // Initialize Hessians - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { diff --git a/src/dynamics_model/lti_system.cpp b/src/dynamics_model/lti_system.cpp index 82118ae9..9b8073c3 100644 --- a/src/dynamics_model/lti_system.cpp +++ b/src/dynamics_model/lti_system.cpp @@ -95,10 +95,7 @@ std::vector LTISystem::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // State Hessian is zero for linear system - std::vector hessians(state_dim_); - for (int i = 0; i < state_dim_; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(state_dim_, state_dim_); - } + auto hessians = makeZeroTensor(state_dim_, state_dim_, state_dim_); return hessians; } @@ -106,10 +103,7 @@ std::vector LTISystem::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // Control Hessian is zero for linear system - std::vector hessians(state_dim_); - for (int i = 0; i < state_dim_; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(control_dim_, control_dim_); - } + auto hessians = makeZeroTensor(state_dim_, control_dim_, control_dim_); return hessians; } diff --git a/src/dynamics_model/manipulator.cpp b/src/dynamics_model/manipulator.cpp index 1c61aa8d..8b65e089 100644 --- a/src/dynamics_model/manipulator.cpp +++ b/src/dynamics_model/manipulator.cpp @@ -73,10 +73,7 @@ std::vector Manipulator::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // For the simplified model, return zero Hessian - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } @@ -84,10 +81,7 @@ std::vector Manipulator::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // For the simplified model, return zero Hessian - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index 4f11306a..cd0c819a 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -84,10 +84,7 @@ std::vector Pendulum::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // Initialize a vector of matrices (one matrix per state dimension) - std::vector hessian(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessian[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessian = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Extract state variables const double theta = state(STATE_THETA); @@ -104,10 +101,7 @@ std::vector Pendulum::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // Initialize a vector of matrices (one matrix per state dimension) - std::vector hessian(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessian = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // For the pendulum, all second derivatives with respect to control are zero // No need to set any values as the matrices are already initialized to zero diff --git a/src/dynamics_model/spacecraft_landing2d.cpp b/src/dynamics_model/spacecraft_landing2d.cpp index f391ab0d..26bbe5cb 100644 --- a/src/dynamics_model/spacecraft_landing2d.cpp +++ b/src/dynamics_model/spacecraft_landing2d.cpp @@ -99,19 +99,13 @@ Eigen::MatrixXd SpacecraftLanding2D::getControlJacobian( std::vector SpacecraftLanding2D::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } std::vector SpacecraftLanding2D::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/spacecraft_linear.cpp b/src/dynamics_model/spacecraft_linear.cpp index dcb7858b..c804f8ce 100644 --- a/src/dynamics_model/spacecraft_linear.cpp +++ b/src/dynamics_model/spacecraft_linear.cpp @@ -108,20 +108,14 @@ Eigen::MatrixXd HCW::getControlJacobian( std::vector HCW::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // HCW equations are linear, so state Hessian is zero - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } std::vector HCW::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { // HCW equations are linear in control, so control Hessian is zero - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/spacecraft_linear_fuel.cpp b/src/dynamics_model/spacecraft_linear_fuel.cpp index baa18c87..0e105eef 100644 --- a/src/dynamics_model/spacecraft_linear_fuel.cpp +++ b/src/dynamics_model/spacecraft_linear_fuel.cpp @@ -140,28 +140,19 @@ Eigen::MatrixXd SpacecraftLinearFuel::getControlJacobian( std::vector SpacecraftLinearFuel::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } std::vector SpacecraftLinearFuel::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } std::vector SpacecraftLinearFuel::getCrossHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/spacecraft_nonlinear.cpp b/src/dynamics_model/spacecraft_nonlinear.cpp index 394cfafc..39cbdedb 100644 --- a/src/dynamics_model/spacecraft_nonlinear.cpp +++ b/src/dynamics_model/spacecraft_nonlinear.cpp @@ -106,10 +106,7 @@ std::vector SpacecraftNonlinear::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } @@ -117,10 +114,7 @@ std::vector SpacecraftNonlinear::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/spacecraft_roe.cpp b/src/dynamics_model/spacecraft_roe.cpp index 4242b300..71fe4704 100644 --- a/src/dynamics_model/spacecraft_roe.cpp +++ b/src/dynamics_model/spacecraft_roe.cpp @@ -179,11 +179,7 @@ namespace cddp const Eigen::VectorXd & /*control*/, double time) const { // For this linear(ish) model, second derivatives wrt state are zero. - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) - { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } @@ -193,11 +189,7 @@ namespace cddp const Eigen::VectorXd & /*control*/, double time) const { // Similarly, second derivatives wrt control are zero for a linear system. - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) - { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/spacecraft_twobody.cpp b/src/dynamics_model/spacecraft_twobody.cpp index 578d5b14..cffc6a3d 100644 --- a/src/dynamics_model/spacecraft_twobody.cpp +++ b/src/dynamics_model/spacecraft_twobody.cpp @@ -62,19 +62,13 @@ Eigen::MatrixXd SpacecraftTwobody::getControlJacobian( std::vector SpacecraftTwobody::getStateHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); return hessians; } std::vector SpacecraftTwobody::getControlHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessians; } diff --git a/src/dynamics_model/unicycle.cpp b/src/dynamics_model/unicycle.cpp index d057d4ae..db51fa3c 100644 --- a/src/dynamics_model/unicycle.cpp +++ b/src/dynamics_model/unicycle.cpp @@ -90,10 +90,7 @@ Eigen::MatrixXd Unicycle::getControlJacobian( std::vector Unicycle::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); // Non-zero elements // Hessian of x w.r.t. theta twice @@ -110,10 +107,7 @@ std::vector Unicycle::getStateHessian( std::vector Unicycle::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - std::vector hessians(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); // No non-zero terms in control Hessian for this model diff --git a/src/dynamics_model/usv_3dof.cpp b/src/dynamics_model/usv_3dof.cpp index e47883fa..b28ee9f3 100644 --- a/src/dynamics_model/usv_3dof.cpp +++ b/src/dynamics_model/usv_3dof.cpp @@ -241,10 +241,7 @@ std::vector Usv3Dof::getControlHessian( { // Dynamics are linear in control (nu_dot = M_inv * (tau - ...)), // so the second derivative d^2f / du^2 is zero. - std::vector hessian(STATE_DIM); - for (int i = 0; i < STATE_DIM; ++i) { - hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); - } + auto hessian = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); return hessian; } From 942aee6ae4b872dc5ce45083cd170e0dc3304ba5 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 11:57:09 -0400 Subject: [PATCH 2/7] Replace CDDPSolution map with typed struct Eliminates all std::any_cast usage across 41 files. Solution fields are now accessed directly (e.g., solution.state_trajectory instead of std::any_cast>(solution.at("state_trajectory"))). --- .claude/setting.json | 50 ++++++++ examples/cddp_acrobot.cpp | 6 +- examples/cddp_bicycle.cpp | 6 +- examples/cddp_car.cpp | 6 +- examples/cddp_car_ipddp.cpp | 6 +- examples/cddp_cartpole.cpp | 6 +- examples/cddp_forklift_ipddp.cpp | 6 +- examples/cddp_hcw.cpp | 8 +- examples/cddp_lti_system.cpp | 14 +- examples/cddp_manipulator.cpp | 6 +- examples/cddp_pendulum.cpp | 4 +- examples/cddp_quadrotor_circle.cpp | 6 +- ...cddp_quadrotor_figure_eight_horizontal.cpp | 6 +- ...quadrotor_figure_eight_horizontal_safe.cpp | 10 +- .../cddp_quadrotor_figure_eight_vertical.cpp | 6 +- examples/cddp_quadrotor_point.cpp | 6 +- examples/cddp_spacecraft_linear_docking.cpp | 8 +- examples/cddp_spacecraft_linear_rpo.cpp | 4 +- examples/cddp_spacecraft_nonlinear_rpo.cpp | 4 +- examples/cddp_spacecraft_roe_rpo.cpp | 4 +- examples/cddp_spacecraft_rpo.cpp | 4 +- examples/cddp_spacecraft_rpo_fuel.cpp | 4 +- examples/cddp_spacecraft_rpo_mc.cpp | 6 +- examples/cddp_unicycle.cpp | 6 +- examples/cddp_unicycle_mpc.cpp | 6 +- examples/cddp_unicycle_safe.cpp | 4 +- examples/cddp_unicycle_safe_ipddp.cpp | 18 +-- examples/cddp_unicycle_safe_ipddp_v2.cpp | 4 +- examples/cddp_unicycle_safe_ipddp_v3.cpp | 4 +- examples/mpc_hcw.cpp | 6 +- examples/test_barrier_strategies.cpp | 15 +-- include/cddp-cpp/cddp_core/cddp_core.hpp | 107 ++++++---------- src/cddp_core/cddp_core.cpp | 20 +-- src/cddp_core/cddp_solver_base.cpp | 61 ++++----- src/cddp_core/ipddp_solver.cpp | 25 ++-- src/cddp_core/logddp_solver.cpp | 6 +- src/cddp_core/msipddp_solver.cpp | 25 ++-- tests/cddp_core/test_cddp_core.cpp | 120 ++++++------------ tests/cddp_core/test_clddp_solver.cpp | 74 +++++------ tests/cddp_core/test_ipddp_solver.cpp | 74 +++++------ tests/cddp_core/test_logddp_solver.cpp | 74 +++++------ tests/cddp_core/test_msipddp_solver.cpp | 74 +++++------ 42 files changed, 425 insertions(+), 484 deletions(-) create mode 100644 .claude/setting.json diff --git a/.claude/setting.json b/.claude/setting.json new file mode 100644 index 00000000..7ef2ce73 --- /dev/null +++ b/.claude/setting.json @@ -0,0 +1,50 @@ +{ + "tools": { + "bash": { + "disabled_commands": [ + "sudo", + "su", + "passwd", + "usermod", + "groupmod", + "adduser", + "useradd", + "deluser", + "userdel", + "chown", + "chmod", + "mount", + "umount", + "systemctl", + "service", + "apt", + "apt-get", + "dpkg", + "snap", + "pip install --system", + "npm install -g" + ] + } + }, + "security": { + "restrict_privileged_operations": true, + "sandbox_mode": true, + "allowed_patterns": [ + "sudo chown .*github/cddp-cpp.*", + "sudo mkdir -p .*github/cddp-cpp.*", + "sudo chmod .*github/cddp-cpp.*", + "pip install --user .*", + "pip install --target .*", + "pip install --prefix .*", + "pip3 install --user .*", + "pip3 install --target .*", + "pip3 install --prefix .*", + "apt search .*", + "apt show .*", + "apt list .*", + "apt-get update", + "npm install .*", + "docker exec .*" + ] + } +} diff --git a/examples/cddp_acrobot.cpp b/examples/cddp_acrobot.cpp index eddd2807..a2d64f95 100644 --- a/examples/cddp_acrobot.cpp +++ b/examples/cddp_acrobot.cpp @@ -105,9 +105,9 @@ int main() { // Solve cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Create plot directory const std::string plotDirectory = "../results/acrobot"; diff --git a/examples/cddp_bicycle.cpp b/examples/cddp_bicycle.cpp index db99a730..dc750ff1 100644 --- a/examples/cddp_bicycle.cpp +++ b/examples/cddp_bicycle.cpp @@ -66,9 +66,9 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Create directory for saving plots const std::string plotDirectory = "../results/tests"; diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp index 52b5deb5..558796d6 100644 --- a/examples/cddp_car.cpp +++ b/examples/cddp_car.cpp @@ -251,9 +251,9 @@ int main() // ======================================== // Extract solution trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Prepare trajectory data std::vector x_hist, y_hist; diff --git a/examples/cddp_car_ipddp.cpp b/examples/cddp_car_ipddp.cpp index 77b3b848..4551d483 100644 --- a/examples/cddp_car_ipddp.cpp +++ b/examples/cddp_car_ipddp.cpp @@ -204,9 +204,9 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); // Extract solution trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Prepare trajectory data for plotting std::vector x_hist, y_hist; diff --git a/examples/cddp_cartpole.cpp b/examples/cddp_cartpole.cpp index f14ce659..d84c2b93 100644 --- a/examples/cddp_cartpole.cpp +++ b/examples/cddp_cartpole.cpp @@ -127,9 +127,9 @@ int main() { // Solve. cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Create plot directory. const std::string plotDirectory = "../results/tests"; diff --git a/examples/cddp_forklift_ipddp.cpp b/examples/cddp_forklift_ipddp.cpp index 1f10c53d..fb0790d6 100644 --- a/examples/cddp_forklift_ipddp.cpp +++ b/examples/cddp_forklift_ipddp.cpp @@ -246,9 +246,9 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); // Extract solution trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Prepare trajectory data for plotting std::vector x_hist, y_hist; diff --git a/examples/cddp_hcw.cpp b/examples/cddp_hcw.cpp index 4d34fedd..9420df5c 100644 --- a/examples/cddp_hcw.cpp +++ b/examples/cddp_hcw.cpp @@ -146,11 +146,11 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); // Extract solution and print result - auto cost_sequence = std::any_cast>(solution.at("cost_trajectory")); + const auto& cost_sequence = solution.history.objective; double J_final = cost_sequence.back(); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::cout << "\n[Result] CDDP solved." << std::endl; std::cout << "[Result] Final cost: " << J_final << std::endl; diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp index c5208199..0d704109 100644 --- a/examples/cddp_lti_system.cpp +++ b/examples/cddp_lti_system.cpp @@ -147,13 +147,13 @@ int main() { // Alternatively: cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::LogDDP); // Extract solution trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); - auto cost_sequence = std::any_cast>(solution.at("cost_trajectory")); - auto iterations = std::any_cast(solution.at("iterations")); - auto converged = std::any_cast(solution.at("converged")); - auto solve_time = std::any_cast(solution.at("solve_time")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; + const auto& cost_sequence = solution.history.objective; + auto iterations = solution.iterations_completed; + bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound"); + auto solve_time = static_cast(solution.solve_time_ms); // Create directory for plots if it doesn't exist const std::string plotDirectory = "../results/tests"; diff --git a/examples/cddp_manipulator.cpp b/examples/cddp_manipulator.cpp index a705e8e2..af7acf2e 100644 --- a/examples/cddp_manipulator.cpp +++ b/examples/cddp_manipulator.cpp @@ -89,9 +89,9 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); // -------------------- Extract Trajectories for Static Plots -------------------- - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::vector time; // for state trajectories std::vector time_ctrl; // for control trajectories diff --git a/examples/cddp_pendulum.cpp b/examples/cddp_pendulum.cpp index 4ab68c60..371d657d 100644 --- a/examples/cddp_pendulum.cpp +++ b/examples/cddp_pendulum.cpp @@ -107,8 +107,8 @@ int main() { // Solve the optimal control problem cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; // Create plot directory if it doesn't exist const std::string plotDirectory = "../results/tests"; diff --git a/examples/cddp_quadrotor_circle.cpp b/examples/cddp_quadrotor_circle.cpp index ef0da961..bfd0d8de 100644 --- a/examples/cddp_quadrotor_circle.cpp +++ b/examples/cddp_quadrotor_circle.cpp @@ -244,9 +244,9 @@ int main() // Solve the optimal control problem (LogDDP, IPDDP, MSIPDDP) cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::cout << "Final state: " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_quadrotor_figure_eight_horizontal.cpp b/examples/cddp_quadrotor_figure_eight_horizontal.cpp index 6a80719f..1e4a97b5 100644 --- a/examples/cddp_quadrotor_figure_eight_horizontal.cpp +++ b/examples/cddp_quadrotor_figure_eight_horizontal.cpp @@ -238,9 +238,9 @@ int main() // Solve the problem cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::cout << "Final state = " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp b/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp index 0d587c09..50863e33 100644 --- a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp +++ b/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp @@ -260,16 +260,16 @@ int main() solver_ball.addPathConstraint("BallConstraint", std::make_unique(ball_radius, ball_center)); // Initial trajectory guess (extract from solution) - auto initial_X = std::any_cast>(solution.at("state_trajectory")); - auto initial_U = std::any_cast>(solution.at("control_trajectory")); + const auto& initial_X = solution.state_trajectory; + const auto& initial_U = solution.control_trajectory; solver_ball.setInitialTrajectory(initial_X, initial_U); // Solve the problem (MSIPDDP, IPDDP) cddp::CDDPSolution solution_ball = solver_ball.solve("MSIPDDP"); - auto X_sol = std::any_cast>(solution_ball.at("state_trajectory")); - auto U_sol = std::any_cast>(solution_ball.at("control_trajectory")); - auto t_sol = std::any_cast>(solution_ball.at("time_points")); + const auto& X_sol = solution_ball.state_trajectory; + const auto& U_sol = solution_ball.control_trajectory; + const auto& t_sol = solution_ball.time_points; std::cout << "Final state = " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_quadrotor_figure_eight_vertical.cpp b/examples/cddp_quadrotor_figure_eight_vertical.cpp index b5008e59..7b19dfd8 100644 --- a/examples/cddp_quadrotor_figure_eight_vertical.cpp +++ b/examples/cddp_quadrotor_figure_eight_vertical.cpp @@ -239,9 +239,9 @@ int main() // Solve the optimal control problem cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::cout << "Final state: " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_quadrotor_point.cpp b/examples/cddp_quadrotor_point.cpp index 8498f0bd..9bb29593 100644 --- a/examples/cddp_quadrotor_point.cpp +++ b/examples/cddp_quadrotor_point.cpp @@ -200,9 +200,9 @@ int main() // Solve the optimal control problem cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; std::cout << "Final state: " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_spacecraft_linear_docking.cpp b/examples/cddp_spacecraft_linear_docking.cpp index 99a8ad65..33ef3a0d 100644 --- a/examples/cddp_spacecraft_linear_docking.cpp +++ b/examples/cddp_spacecraft_linear_docking.cpp @@ -127,7 +127,7 @@ int main() { // Solve the unconstrained problem (e.g., using IPDDP) cddp::CDDPSolution initial_solution = cddp_solver_init.solve("IPDDP"); - auto initial_states = std::any_cast>(initial_solution.at("state_trajectory")); + const auto& initial_states = initial_solution.state_trajectory; if (initial_states.empty()) { std::cerr << "Failed to find an initial guess solution. Exiting." << std::endl; return 1; @@ -172,8 +172,8 @@ int main() { // Initialize trajectory guess // Use the solution from the unconstrained solve as the initial guess - std::vector X_init = std::any_cast>(initial_solution.at("state_trajectory")); - std::vector U_init = std::any_cast>(initial_solution.at("control_trajectory")); + const auto& X_init = initial_solution.state_trajectory; + const auto& U_init = initial_solution.control_trajectory; // Assign the initial trajectory guess to the solver cddp_solver.setInitialTrajectory(X_init, U_init); @@ -185,7 +185,7 @@ int main() { // ========================================================================= // 3) Visualize the Trajectory // ========================================================================= - auto solution_states = std::any_cast>(solution.at("state_trajectory")); + const auto& solution_states = solution.state_trajectory; if (!solution_states.empty()) { namespace plt = matplot; diff --git a/examples/cddp_spacecraft_linear_rpo.cpp b/examples/cddp_spacecraft_linear_rpo.cpp index 0caa9e7c..85886d14 100644 --- a/examples/cddp_spacecraft_linear_rpo.cpp +++ b/examples/cddp_spacecraft_linear_rpo.cpp @@ -216,8 +216,8 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract the solution - std::vector X_solution = std::any_cast>(solution.at("state_trajectory")); - std::vector U_solution = std::any_cast>(solution.at("control_trajectory")); + const auto& X_solution = solution.state_trajectory; + const auto& U_solution = solution.control_trajectory; if (!X_solution.empty() && !U_solution.empty()) { diff --git a/examples/cddp_spacecraft_nonlinear_rpo.cpp b/examples/cddp_spacecraft_nonlinear_rpo.cpp index 1b611e01..e127f8ae 100644 --- a/examples/cddp_spacecraft_nonlinear_rpo.cpp +++ b/examples/cddp_spacecraft_nonlinear_rpo.cpp @@ -227,8 +227,8 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract the solution - std::vector X_solution = std::any_cast>(solution.at("state_trajectory")); - std::vector U_solution = std::any_cast>(solution.at("control_trajectory")); + const auto& X_solution = solution.state_trajectory; + const auto& U_solution = solution.control_trajectory; if (!X_solution.empty() && !U_solution.empty()) { diff --git a/examples/cddp_spacecraft_roe_rpo.cpp b/examples/cddp_spacecraft_roe_rpo.cpp index f2d934cc..7420a895 100644 --- a/examples/cddp_spacecraft_roe_rpo.cpp +++ b/examples/cddp_spacecraft_roe_rpo.cpp @@ -322,8 +322,8 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract the solution - std::vector X_solution_roe = std::any_cast>(solution.at("state_trajectory")); - std::vector U_solution_accel = std::any_cast>(solution.at("control_trajectory")); + const auto& X_solution_roe = solution.state_trajectory; + const auto& U_solution_accel = solution.control_trajectory; // Print the solution // std::cout << "Solution: " << solution.cost << std::endl; diff --git a/examples/cddp_spacecraft_rpo.cpp b/examples/cddp_spacecraft_rpo.cpp index 47c586e5..3c95dd12 100644 --- a/examples/cddp_spacecraft_rpo.cpp +++ b/examples/cddp_spacecraft_rpo.cpp @@ -216,8 +216,8 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract the solution - std::vector X_solution = std::any_cast>(solution.at("state_trajectory")); - std::vector U_solution = std::any_cast>(solution.at("control_trajectory")); + const auto& X_solution = solution.state_trajectory; + const auto& U_solution = solution.control_trajectory; if (!X_solution.empty() && !U_solution.empty()) { diff --git a/examples/cddp_spacecraft_rpo_fuel.cpp b/examples/cddp_spacecraft_rpo_fuel.cpp index e4a3cb09..b769dbd8 100644 --- a/examples/cddp_spacecraft_rpo_fuel.cpp +++ b/examples/cddp_spacecraft_rpo_fuel.cpp @@ -241,8 +241,8 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract the solution - std::vector X_solution = std::any_cast>(solution.at("state_trajectory")); - std::vector U_solution = std::any_cast>(solution.at("control_trajectory")); + const auto& X_solution = solution.state_trajectory; + const auto& U_solution = solution.control_trajectory; if (!X_solution.empty() && !U_solution.empty()) { diff --git a/examples/cddp_spacecraft_rpo_mc.cpp b/examples/cddp_spacecraft_rpo_mc.cpp index 667210db..23454487 100644 --- a/examples/cddp_spacecraft_rpo_mc.cpp +++ b/examples/cddp_spacecraft_rpo_mc.cpp @@ -231,9 +231,9 @@ int main() // Solve the Trajectory Optimization Problem cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); - auto state_trajectory = std::any_cast>(solution.at("state_trajectory")); - auto control_trajectory = std::any_cast>(solution.at("control_trajectory")); - bool converged = std::any_cast(solution.at("converged")); + const auto& state_trajectory = solution.state_trajectory; + const auto& control_trajectory = solution.control_trajectory; + bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound"); if (!state_trajectory.empty() && !control_trajectory.empty() && converged) { diff --git a/examples/cddp_unicycle.cpp b/examples/cddp_unicycle.cpp index 839c93a7..1818fc05 100644 --- a/examples/cddp_unicycle.cpp +++ b/examples/cddp_unicycle.cpp @@ -82,9 +82,9 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); // size: horizon + 1 - auto U_sol = std::any_cast>(solution.at("control_trajectory")); // size: horizon - auto t_sol = std::any_cast>(solution.at("time_points")); // size: horizon + 1 + const auto& X_sol = solution.state_trajectory; // size: horizon + 1 + const auto& U_sol = solution.control_trajectory; // size: horizon + const auto& t_sol = solution.time_points; // size: horizon + 1 // Create directory for saving plot (if it doesn't exist) const std::string plotDirectory = "../results/tests"; diff --git a/examples/cddp_unicycle_mpc.cpp b/examples/cddp_unicycle_mpc.cpp index 261af7d6..06117fa2 100644 --- a/examples/cddp_unicycle_mpc.cpp +++ b/examples/cddp_unicycle_mpc.cpp @@ -172,14 +172,14 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Extract and apply the first control - auto status = std::any_cast(solution.at("status_message")); + const auto& status = solution.status_message; if (status != "OptimalSolutionFound" && status != "AcceptableSolutionFound") { std::cerr << "Warning: Solver did not converge at step " << k << ". Status: " << status << std::endl; // Handle non-convergence, e.g., by applying zero control or previous control } - auto U_sol = std::any_cast>(solution.at("control_trajectory")); + const auto& U_sol = solution.control_trajectory; Eigen::VectorXd control_to_apply = U_sol[0]; // Propagate system dynamics @@ -192,7 +192,7 @@ int main() time_history.push_back(current_time); // Warm start for the next iteration: shift the solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); + const auto& X_sol = solution.state_trajectory; for(int i = 0; i < mpc_horizon -1; ++i) { X_guess[i] = X_sol[i+1]; diff --git a/examples/cddp_unicycle_safe.cpp b/examples/cddp_unicycle_safe.cpp index 50f9e924..12c89162 100644 --- a/examples/cddp_unicycle_safe.cpp +++ b/examples/cddp_unicycle_safe.cpp @@ -103,7 +103,7 @@ int main() { // Solve cddp::CDDPSolution solution_baseline = solver_baseline.solve(cddp::SolverType::IPDDP); - auto X_baseline_sol = std::any_cast>(solution_baseline.at("state_trajectory")); // size horizon + 1 + const auto& X_baseline_sol = solution_baseline.state_trajectory; // size horizon + 1 // ------------------------------------------------------- // 3. Solve with BallConstraint @@ -133,7 +133,7 @@ int main() { // Solve cddp::CDDPSolution solution_ball = solver_ball.solve(cddp::SolverType::IPDDP); - auto X_ball_sol = std::any_cast>(solution_ball.at("state_trajectory")); // horizon+1 + const auto& X_ball_sol = solution_ball.state_trajectory; // horizon+1 // ------------------------------------------------------- // 4. Prepare Data for Plotting diff --git a/examples/cddp_unicycle_safe_ipddp.cpp b/examples/cddp_unicycle_safe_ipddp.cpp index 48d77463..17602537 100644 --- a/examples/cddp_unicycle_safe_ipddp.cpp +++ b/examples/cddp_unicycle_safe_ipddp.cpp @@ -116,9 +116,9 @@ int main() { // Solve cddp::CDDPSolution solution_baseline = solver_baseline.solve(cddp::SolverType::MSIPDDP); - auto X_baseline_sol = std::any_cast>(solution_baseline.at("state_trajectory")); // horizon+1 - auto U_baseline_sol = std::any_cast>(solution_baseline.at("control_trajectory")); // horizon - auto T_baseline_sol = std::any_cast>(solution_baseline.at("time_points")); // horizon+1 + const auto& X_baseline_sol = solution_baseline.state_trajectory; // horizon+1 + const auto& U_baseline_sol = solution_baseline.control_trajectory; // horizon + const auto& T_baseline_sol = solution_baseline.time_points; // horizon+1 // -------------------------- // 3. Solve - WITH Ball constraint (naive init) @@ -148,9 +148,9 @@ int main() { // Solve cddp::CDDPSolution solution_ball = solver_ball.solve(cddp::SolverType::MSIPDDP); - auto X_ball_sol = std::any_cast>(solution_ball.at("state_trajectory")); - auto U_ball_sol = std::any_cast>(solution_ball.at("control_trajectory")); - auto T_ball_sol = std::any_cast>(solution_ball.at("time_points")); + const auto& X_ball_sol = solution_ball.state_trajectory; + const auto& U_ball_sol = solution_ball.control_trajectory; + const auto& T_ball_sol = solution_ball.time_points; // -------------------------- // 4. Solve - WITH Ball constraint (baseline init) @@ -178,9 +178,9 @@ int main() { // Solve cddp::CDDPSolution solution_ball_with_baseline = solver_ball_with_baseline.solve(cddp::SolverType::IPDDP); - auto X_ball_with_baseline_sol = std::any_cast>(solution_ball_with_baseline.at("state_trajectory")); - auto U_ball_with_baseline_sol = std::any_cast>(solution_ball_with_baseline.at("control_trajectory")); - auto T_ball_with_baseline_sol = std::any_cast>(solution_ball_with_baseline.at("time_points")); + const auto& X_ball_with_baseline_sol = solution_ball_with_baseline.state_trajectory; + const auto& U_ball_with_baseline_sol = solution_ball_with_baseline.control_trajectory; + const auto& T_ball_with_baseline_sol = solution_ball_with_baseline.time_points; // -------------------------- // 5. Convert solutions to std::vectors for plotting diff --git a/examples/cddp_unicycle_safe_ipddp_v2.cpp b/examples/cddp_unicycle_safe_ipddp_v2.cpp index 2a7db418..7191ad3d 100644 --- a/examples/cddp_unicycle_safe_ipddp_v2.cpp +++ b/examples/cddp_unicycle_safe_ipddp_v2.cpp @@ -118,8 +118,8 @@ int main() { // Solve cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - X_sol = std::any_cast>(solution.at("state_trajectory")); - U_sol = std::any_cast>(solution.at("control_trajectory")); + X_sol = solution.state_trajectory; + U_sol = solution.control_trajectory; // ------------------------------------------------- // 3. Prepare Data for Plotting diff --git a/examples/cddp_unicycle_safe_ipddp_v3.cpp b/examples/cddp_unicycle_safe_ipddp_v3.cpp index bb986fb0..cdc908db 100644 --- a/examples/cddp_unicycle_safe_ipddp_v3.cpp +++ b/examples/cddp_unicycle_safe_ipddp_v3.cpp @@ -127,8 +127,8 @@ int main() { // Solve cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP); - X_sol = std::any_cast>(solution.at("state_trajectory")); - U_sol = std::any_cast>(solution.at("control_trajectory")); + X_sol = solution.state_trajectory; + U_sol = solution.control_trajectory; // Add this line to check the actual starting state in the solution std::cout << "Actual starting state in solution: " << X_sol[0].transpose() << std::endl; diff --git a/examples/mpc_hcw.cpp b/examples/mpc_hcw.cpp index 7163e2c4..af757f2c 100644 --- a/examples/mpc_hcw.cpp +++ b/examples/mpc_hcw.cpp @@ -260,11 +260,11 @@ int main() { cddp::CDDPSolution sol = cddp_solver.solve(); // Extract the *first* control from that horizon - auto control_traj = std::any_cast>(sol.at("control_trajectory")); + const auto& control_traj = sol.control_trajectory; current_u = control_traj[0]; - auto X_sequence = std::any_cast>(sol.at("state_trajectory")); - auto U_sequence = std::any_cast>(sol.at("control_trajectory")); + const auto& X_sequence = sol.state_trajectory; + const auto& U_sequence = sol.control_trajectory; cddp_solver.setInitialTrajectory(X_sequence, U_sequence); } diff --git a/examples/test_barrier_strategies.cpp b/examples/test_barrier_strategies.cpp index 7247dbe5..6ca19e3e 100644 --- a/examples/test_barrier_strategies.cpp +++ b/examples/test_barrier_strategies.cpp @@ -124,10 +124,10 @@ int main() cddp::CDDPSolution solution = cddp_solver.solve(solver_name); // Print results - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations = solution.iterations_completed; + auto solve_time = solution.solve_time_ms; + auto final_cost = solution.final_objective; std::cout << "Status: " << status_message << "\n"; std::cout << "Iterations: " << iterations << "\n"; @@ -135,12 +135,7 @@ int main() std::cout << "Solve time: " << solve_time << " ms\n"; // Extract final barrier parameter - try { - double final_mu = std::any_cast(solution.at("final_barrier_parameter_mu")); - std::cout << "Final barrier μ: " << final_mu << "\n"; - } catch (const std::exception& e) { - std::cout << "Final barrier μ: Not available\n"; - } + std::cout << "Final barrier μ: " << solution.final_barrier_mu << "\n"; } } diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index 1f775b81..9a0bdaa1 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -17,7 +17,7 @@ #define CDDP_CDDP_CORE_HPP #include -#include // For std::any +#include // For std::any (kept for backward compat in printSolutionSummary) #include #include // For std::setw #include // For std::cout, std::cerr @@ -50,76 +50,43 @@ enum class SolverType { }; /** - * @brief Solution data from the CDDP solver, as a map of string keys to - * `std::any` values. - * - * Retrieve values using `std::any_cast(solution.at("key"))`. - * Always check `solution.count("key")` before `.at()` to prevent - * `std::out_of_range`. Handle `std::bad_any_cast` for type mismatches. Optional - * keys are present only if computed by the specific solver. - * - * --- General Information --- - * - "solver_name": std::string (Name of the solver used, - * e.g., "IPDDP") - * - "status_message": std::string (Termination status; possible - * values:) • "OptimalSolutionFound" - Converged to optimal solution • - * "AcceptableSolutionFound" - Reached cost tolerance limit • - * "MaxIterationsReached" - Reached maximum iteration limit • - * "MaxCpuTimeReached" - Exceeded maximum CPU time limit • - * "RegularizationLimitReached_Converged" - Reached regularization limit but - * solution acceptable • "RegularizationLimitReached_NotConverged" - Reached - * regularization limit, solution not acceptable - * - "iterations_completed": int (Number of iterations) - * - "solve_time_ms": double (Total solver time in milliseconds) - * - "final_objective": double (Final objective cost J(x,u)) - * - "final_step_length": double (Final line search step size; use - * "final_step_length_primal" / "_dual" if distinct) - * - * --- Primary Solution Trajectories --- - * - "time_points": std::vector (Time points t_0..t_N) - * - "state_trajectory": std::vector (States - * X_0..X_N) - * - "control_trajectory": std::vector (Controls - * U_0..U_{N-1}) - * - * --- Iteration History (Optional) --- - * (Vectors indexed by iteration number) - * - "history_objective": std::vector (Objective J) - * - "history_merit_function": std::vector (Merit function value, - * e.g., for IPMs) - * - "history_primal_infeasibility": std::vector (Primal constraint - * violation norm (inf_pr), including dynamics defects) - * - "history_dual_infeasibility": std::vector (Lagrangian gradient - * norm (inf_du) or other dual infeasibility) - * - "history_complementary_infeasibility": std::vector (Complementary - * infeasibility) - * - "history_barrier_mu": std::vector (Barrier parameter - * (mu) for IPMs; user can log10 for "lg(mu)") - * - "history_regularization": std::map> - * (Regularization values; user can log10 for "lg(rg)") - * - "history_step_length_primal": std::vector (Primal step length - * (alpha_pr)) - * - "history_step_length_dual": std::vector (Dual step length - * (alpha_du)) - * - "history_linesearch_iterations": std::vector (Line search - * sub-iterations (ls)) - * - * --- Final Metrics (at termination, Optional) --- - * - "final_primal_infeasibility": double (Total primal constraint violation - * norm, including dynamics defects) - * - "final_dual_infeasibility": double (Lagrangian gradient norm or other - * dual infeasibility metric) - * - * --- Controller Gains (Feedback Policy, Optional) --- - * - "control_feedback_gains_k": std::vector (Feedback gains - * K_u) - * - * --- Solver-Specific Internal Metrics (at termination, Optional) --- - * - "final_barrier_parameter_mu": double (Barrier parameter mu for IPMs; - * smallness implies complementarity) - * - "final_regularization": double (Final regularization value) + * @brief Solution data from the CDDP solver. */ -using CDDPSolution = std::map; +struct CDDPSolution { + // --- General Information --- + std::string solver_name; + std::string status_message = "Running"; + int iterations_completed = 0; + double solve_time_ms = 0.0; + double final_objective = 0.0; + double final_step_length = 0.0; + double final_regularization = 0.0; + + // --- Primary Solution --- + std::vector time_points; + std::vector state_trajectory; + std::vector control_trajectory; + std::vector feedback_gains; + + // --- Final Metrics (IP solvers) --- + double final_primal_infeasibility = 0.0; + double final_dual_infeasibility = 0.0; + double final_complementary_infeasibility = 0.0; + double final_barrier_mu = 0.0; + + // --- Iteration History (populated if return_iteration_info) --- + struct History { + std::vector objective; + std::vector merit_function; + std::vector step_length_primal; + std::vector step_length_dual; + std::vector dual_infeasibility; + std::vector primal_infeasibility; + std::vector complementary_infeasibility; + std::vector barrier_mu; + std::vector regularization; + } history; +}; struct ForwardPassResult { // Core trajectories always computed in a forward pass diff --git a/src/cddp_core/cddp_core.cpp b/src/cddp_core/cddp_core.cpp index c2ffe7e4..65a3cf8c 100644 --- a/src/cddp_core/cddp_core.cpp +++ b/src/cddp_core/cddp_core.cpp @@ -306,19 +306,13 @@ CDDPSolution CDDP::solve(const std::string &solver_type) { if (!solver_) { // Solver not found - return error solution CDDPSolution solution; - solution["solver_name"] = solver_type; - solution["status_message"] = - std::string("UnknownSolver - No solver registered for '") + - solver_type + "'"; - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; - solution["final_objective"] = 0.0; - solution["final_step_length"] = 1.0; - - // Add empty trajectories - solution["time_points"] = std::vector(); - solution["state_trajectory"] = std::vector(); - solution["control_trajectory"] = std::vector(); + solution.solver_name = solver_type; + solution.status_message = + "UnknownSolver - No solver registered for '" + solver_type + "'"; + solution.iterations_completed = 0; + solution.solve_time_ms = 0.0; + solution.final_objective = 0.0; + solution.final_step_length = 1.0; if (options_.verbose) { std::cout << "Solver type '" << solver_type diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index 1f282a8e..569eca32 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -64,10 +64,10 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { // Prepare solution CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("Running"); - solution["iterations_completed"] = 0; - solution["solve_time_ms"] = 0.0; + solution.solver_name = getSolverName(); + solution.status_message = "Running"; + solution.iterations_completed = 0; + solution.solve_time_ms = 0.0; // Initialize history if (options.return_iteration_info) { @@ -176,31 +176,29 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { end_time - start_time); // Populate common solution fields - solution["status_message"] = termination_reason; - solution["iterations_completed"] = iter; - solution["solve_time_ms"] = static_cast(duration.count()); - solution["final_objective"] = context.cost_; - solution["final_step_length"] = context.alpha_pr_; - solution["time_points"] = buildTimePoints(context); - solution["state_trajectory"] = context.X_; - solution["control_trajectory"] = context.U_; - solution["control_feedback_gains_K"] = K_u_; - solution["final_regularization"] = context.regularization_; + solution.status_message = termination_reason; + solution.iterations_completed = iter; + solution.solve_time_ms = static_cast(duration.count()); + solution.final_objective = context.cost_; + solution.final_step_length = context.alpha_pr_; + solution.time_points = buildTimePoints(context); + solution.state_trajectory = context.X_; + solution.control_trajectory = context.U_; + solution.feedback_gains = K_u_; + solution.final_regularization = context.regularization_; // Add iteration history if requested if (options.return_iteration_info) { - solution["history_objective"] = history_.objective; - solution["history_merit_function"] = history_.merit_function; - solution["history_step_length_primal"] = history_.step_length_primal; - solution["history_step_length_dual"] = history_.step_length_dual; - solution["history_dual_infeasibility"] = history_.dual_infeasibility; - solution["history_primal_infeasibility"] = history_.primal_infeasibility; - solution["history_complementary_infeasibility"] = + solution.history.objective = history_.objective; + solution.history.merit_function = history_.merit_function; + solution.history.step_length_primal = history_.step_length_primal; + solution.history.step_length_dual = history_.step_length_dual; + solution.history.dual_infeasibility = history_.dual_infeasibility; + solution.history.primal_infeasibility = history_.primal_infeasibility; + solution.history.complementary_infeasibility = history_.complementary_infeasibility; - solution["history_barrier_mu"] = history_.barrier_mu; - std::map> history_regularization_map; - history_regularization_map["control"] = history_.regularization; - solution["history_regularization"] = history_regularization_map; + solution.history.barrier_mu = history_.barrier_mu; + solution.history.regularization = history_.regularization; } // Solver-specific solution fields @@ -255,15 +253,10 @@ void CDDPSolverBase::printSolutionSummary(const CDDPSolution &solution) const { std::cout << " " << getSolverName() << " Solution Summary\n"; std::cout << "========================================\n"; - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); - auto status = std::any_cast(solution.at("status_message")); - - std::cout << "Status: " << status << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; - std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; + std::cout << "Status: " << solution.status_message << "\n"; + std::cout << "Iterations: " << solution.iterations_completed << "\n"; + std::cout << "Solve Time: " << std::setprecision(2) << solution.solve_time_ms << " ms\n"; + std::cout << "Final Cost: " << std::setprecision(6) << solution.final_objective << "\n"; std::cout << "========================================\n\n"; } diff --git a/src/cddp_core/ipddp_solver.cpp b/src/cddp_core/ipddp_solver.cpp index 0648aacd..3f88209c 100644 --- a/src/cddp_core/ipddp_solver.cpp +++ b/src/cddp_core/ipddp_solver.cpp @@ -330,10 +330,10 @@ namespace cddp void IPDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, const CDDP &context) { - solution["final_barrier_parameter_mu"] = mu_; - solution["final_primal_infeasibility"] = context.inf_pr_; - solution["final_dual_infeasibility"] = context.inf_du_; - solution["final_complementary_infeasibility"] = context.inf_comp_; + solution.final_barrier_mu = mu_; + solution.final_primal_infeasibility = context.inf_pr_; + solution.final_dual_infeasibility = context.inf_du_; + solution.final_complementary_infeasibility = context.inf_comp_; } void IPDDPSolver::printIteration(int iter, const CDDP &context) const @@ -1452,19 +1452,12 @@ namespace cddp std::cout << " IPDDP Solution Summary\n"; std::cout << "========================================\n"; - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); - auto status = std::any_cast(solution.at("status_message")); - auto final_mu = - std::any_cast(solution.at("final_barrier_parameter_mu")); - - std::cout << "Status: " << status << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; - std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; + std::cout << "Status: " << solution.status_message << "\n"; + std::cout << "Iterations: " << solution.iterations_completed << "\n"; + std::cout << "Solve Time: " << std::setprecision(2) << solution.solve_time_ms << " ms\n"; + std::cout << "Final Cost: " << std::setprecision(6) << solution.final_objective << "\n"; std::cout << "Final Barrier μ: " << std::setprecision(2) << std::scientific - << final_mu << "\n"; + << solution.final_barrier_mu << "\n"; std::cout << "========================================\n\n"; } diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index b8f54b07..b3d79f56 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -278,9 +278,9 @@ void LogDDPSolver::recordIterationHistory(const CDDP &context) { void LogDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, const CDDP &context) { - solution["final_barrier_parameter_mu"] = mu_; - solution["final_primal_infeasibility"] = constraint_violation_; - solution["final_dual_infeasibility"] = context.inf_du_; + solution.final_barrier_mu = mu_; + solution.final_primal_infeasibility = constraint_violation_; + solution.final_dual_infeasibility = context.inf_du_; } void LogDDPSolver::printIteration(int iter, const CDDP &context) const { diff --git a/src/cddp_core/msipddp_solver.cpp b/src/cddp_core/msipddp_solver.cpp index 54a4a465..3067d5c1 100644 --- a/src/cddp_core/msipddp_solver.cpp +++ b/src/cddp_core/msipddp_solver.cpp @@ -405,10 +405,10 @@ namespace cddp void MSIPDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, const CDDP &context) { - solution["final_barrier_parameter_mu"] = mu_; - solution["final_primal_infeasibility"] = context.inf_pr_; - solution["final_dual_infeasibility"] = context.inf_du_; - solution["final_complementary_infeasibility"] = context.inf_comp_; + solution.final_barrier_mu = mu_; + solution.final_primal_infeasibility = context.inf_pr_; + solution.final_dual_infeasibility = context.inf_du_; + solution.final_complementary_infeasibility = context.inf_comp_; } void MSIPDDPSolver::printIteration(int iter, const CDDP &context) const @@ -1837,19 +1837,12 @@ namespace cddp std::cout << " MSIPDDP Solution Summary\n"; std::cout << "========================================\n"; - auto iterations = std::any_cast(solution.at("iterations_completed")); - auto solve_time = std::any_cast(solution.at("solve_time_ms")); - auto final_cost = std::any_cast(solution.at("final_objective")); - auto status = std::any_cast(solution.at("status_message")); - auto final_mu = - std::any_cast(solution.at("final_barrier_parameter_mu")); - - std::cout << "Status: " << status << "\n"; - std::cout << "Iterations: " << iterations << "\n"; - std::cout << "Solve Time: " << std::setprecision(2) << solve_time << " ms\n"; - std::cout << "Final Cost: " << std::setprecision(6) << final_cost << "\n"; + std::cout << "Status: " << solution.status_message << "\n"; + std::cout << "Iterations: " << solution.iterations_completed << "\n"; + std::cout << "Solve Time: " << std::setprecision(2) << solution.solve_time_ms << " ms\n"; + std::cout << "Final Cost: " << std::setprecision(6) << solution.final_objective << "\n"; std::cout << "Final Barrier μ: " << std::setprecision(2) << std::scientific - << final_mu << "\n"; + << solution.final_barrier_mu << "\n"; std::cout << "========================================\n\n"; } diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index 23dbee56..ce3f8a18 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -45,36 +45,29 @@ class MockExternalSolver : public ISolverAlgorithm { solve_called_ = true; CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("OptimalSolutionFound"); - solution["iterations_completed"] = 5; - solution["solve_time_ms"] = 100.0; - solution["final_objective"] = 1.23; - solution["final_step_length"] = 1.0; - + solution.solver_name = getSolverName(); + solution.status_message = "OptimalSolutionFound"; + solution.iterations_completed = 5; + solution.solve_time_ms = 100.0; + solution.final_objective = 1.23; + solution.final_step_length = 1.0; + // Create simple trajectories that match the actual format - std::vector time_points; - time_points.reserve(static_cast(context.getHorizon() + 1)); + solution.time_points.reserve(static_cast(context.getHorizon() + 1)); for (int t = 0; t <= context.getHorizon(); ++t) { - time_points.push_back(t * context.getTimestep()); + solution.time_points.push_back(t * context.getTimestep()); } - - std::vector state_trajectory; - std::vector control_trajectory; - state_trajectory.reserve(static_cast(context.getHorizon() + 1)); - control_trajectory.reserve(static_cast(context.getHorizon())); - + + solution.state_trajectory.reserve(static_cast(context.getHorizon() + 1)); + solution.control_trajectory.reserve(static_cast(context.getHorizon())); + for (int k = 0; k <= context.getHorizon(); ++k) { - state_trajectory.push_back(Eigen::VectorXd::Zero(context.getStateDim())); + solution.state_trajectory.push_back(Eigen::VectorXd::Zero(context.getStateDim())); } for (int k = 0; k < context.getHorizon(); ++k) { - control_trajectory.push_back(Eigen::VectorXd::Zero(context.getControlDim())); + solution.control_trajectory.push_back(Eigen::VectorXd::Zero(context.getControlDim())); } - solution["time_points"] = time_points; - solution["state_trajectory"] = state_trajectory; - solution["control_trajectory"] = control_trajectory; - return solution; } @@ -102,17 +95,12 @@ class AnotherMockSolver : public ISolverAlgorithm { CDDPSolution solve(CDDP &context) override { CDDPSolution solution; - solution["solver_name"] = getSolverName(); - solution["status_message"] = std::string("MaxIterationsReached"); - solution["iterations_completed"] = 10; - solution["solve_time_ms"] = 200.0; - solution["final_objective"] = 4.56; - solution["final_step_length"] = 0.5; - - // Empty trajectories for simplicity - solution["time_points"] = std::vector(); - solution["state_trajectory"] = std::vector(); - solution["control_trajectory"] = std::vector(); + solution.solver_name = getSolverName(); + solution.status_message = "MaxIterationsReached"; + solution.iterations_completed = 10; + solution.solve_time_ms = 200.0; + solution.final_objective = 4.56; + solution.final_step_length = 0.5; return solution; } @@ -231,17 +219,10 @@ TEST_F(CDDPCoreTest, UseRegisteredExternalSolver) { auto solution = cddp_solver.solve("MockExternalSolver"); // Verify the solution came from our mock solver - ASSERT_TRUE(solution.count("solver_name")); - EXPECT_EQ(std::any_cast(solution["solver_name"]), "MockExternalSolver"); - - ASSERT_TRUE(solution.count("status_message")); - EXPECT_EQ(std::any_cast(solution["status_message"]), "OptimalSolutionFound"); - - ASSERT_TRUE(solution.count("iterations_completed")); - EXPECT_EQ(std::any_cast(solution["iterations_completed"]), 5); - - ASSERT_TRUE(solution.count("final_objective")); - EXPECT_DOUBLE_EQ(std::any_cast(solution["final_objective"]), 1.23); + EXPECT_EQ(solution.solver_name, "MockExternalSolver"); + EXPECT_EQ(solution.status_message, "OptimalSolutionFound"); + EXPECT_EQ(solution.iterations_completed, 5); + EXPECT_DOUBLE_EQ(solution.final_objective, 1.23); } // Test built-in solver still works @@ -260,13 +241,9 @@ TEST_F(CDDPCoreTest, BuiltInSolverStillWorks) { auto solution = cddp_solver.solve("CLDDP"); // Verify we get a valid solution (might not converge in 5 iterations, but should run) - ASSERT_TRUE(solution.count("solver_name")); - EXPECT_EQ(std::any_cast(solution["solver_name"]), "CLDDP"); - - ASSERT_TRUE(solution.count("status_message")); + EXPECT_EQ(solution.solver_name, "CLDDP"); // Should have a valid status message - std::string status = std::any_cast(solution["status_message"]); - EXPECT_FALSE(status.empty()); + EXPECT_FALSE(solution.status_message.empty()); } // Test error handling for unknown solver @@ -285,16 +262,10 @@ TEST_F(CDDPCoreTest, UnknownSolverErrorHandling) { auto solution = cddp_solver.solve("NonExistentSolver"); // Verify we get an appropriate error response - ASSERT_TRUE(solution.count("solver_name")); - EXPECT_EQ(std::any_cast(solution["solver_name"]), "NonExistentSolver"); - - ASSERT_TRUE(solution.count("status_message")); - std::string status = std::any_cast(solution["status_message"]); - EXPECT_THAT(status, ::testing::HasSubstr("UnknownSolver")); - EXPECT_THAT(status, ::testing::HasSubstr("NonExistentSolver")); - - ASSERT_TRUE(solution.count("iterations_completed")); - EXPECT_EQ(std::any_cast(solution["iterations_completed"]), 0); + EXPECT_EQ(solution.solver_name, "NonExistentSolver"); + EXPECT_THAT(solution.status_message, ::testing::HasSubstr("UnknownSolver")); + EXPECT_THAT(solution.status_message, ::testing::HasSubstr("NonExistentSolver")); + EXPECT_EQ(solution.iterations_completed, 0); } // Test solver precedence (external over built-in) @@ -316,11 +287,8 @@ TEST_F(CDDPCoreTest, SolverPrecedence) { auto solution = cddp_solver.solve("CLDDP"); // Verify we got the external solver (MockExternalSolver), not built-in CLDDP - ASSERT_TRUE(solution.count("solver_name")); - EXPECT_EQ(std::any_cast(solution["solver_name"]), "MockExternalSolver"); - - ASSERT_TRUE(solution.count("final_objective")); - EXPECT_DOUBLE_EQ(std::any_cast(solution["final_objective"]), 1.23); // Mock solver value + EXPECT_EQ(solution.solver_name, "MockExternalSolver"); + EXPECT_DOUBLE_EQ(solution.final_objective, 1.23); // Mock solver value } // Test enum-based solve still works @@ -339,11 +307,9 @@ TEST_F(CDDPCoreTest, EnumBasedSolveStillWorks) { auto solution = cddp_solver.solve(cddp::SolverType::CLDDP); // Verify we get a valid solution - ASSERT_TRUE(solution.count("solver_name")); // Note: If we registered "CLDDP" above, this might be "MockExternalSolver" // But the enum interface should still work - std::string solver_name = std::any_cast(solution["solver_name"]); - EXPECT_FALSE(solver_name.empty()); + EXPECT_FALSE(solution.solver_name.empty()); } // Test integration with trajectory and options @@ -381,18 +347,8 @@ TEST_F(CDDPCoreTest, IntegrationWithTrajectoryAndOptions) { auto solution = cddp_solver.solve("IntegrationTestSolver"); // Verify solution structure is correct - ASSERT_TRUE(solution.count("solver_name")); - EXPECT_EQ(std::any_cast(solution["solver_name"]), "MockExternalSolver"); - - ASSERT_TRUE(solution.count("time_points")); - auto time_points = std::any_cast>(solution["time_points"]); - EXPECT_EQ(time_points.size(), horizon + 1); - - ASSERT_TRUE(solution.count("state_trajectory")); - auto state_traj = std::any_cast>(solution["state_trajectory"]); - EXPECT_EQ(state_traj.size(), horizon + 1); - - ASSERT_TRUE(solution.count("control_trajectory")); - auto control_traj = std::any_cast>(solution["control_trajectory"]); - EXPECT_EQ(control_traj.size(), horizon); + EXPECT_EQ(solution.solver_name, "MockExternalSolver"); + EXPECT_EQ(solution.time_points.size(), horizon + 1); + EXPECT_EQ(solution.state_trajectory.size(), horizon + 1); + EXPECT_EQ(solution.control_trajectory.size(), horizon); } \ No newline at end of file diff --git a/tests/cddp_core/test_clddp_solver.cpp b/tests/cddp_core/test_clddp_solver.cpp index 717a9318..8e6cfb4d 100644 --- a/tests/cddp_core/test_clddp_solver.cpp +++ b/tests/cddp_core/test_clddp_solver.cpp @@ -123,10 +123,10 @@ TEST(CLDDPTest, SolvePendulum) cddp::CDDPSolution solution = cddp_solver.solve("CLDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -136,9 +136,9 @@ TEST(CLDDPTest, SolvePendulum) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -191,10 +191,10 @@ TEST(CLDDPTest, SolvePendulum) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -294,13 +294,13 @@ TEST(CLDDPTest, SolveUnicycle) { cddp::CDDPSolution solution = cddp_solver.solve("CLDDP"); // cddp::CDDPSolution solution = cddp_solver.solveCLDDP(); - auto status = std::any_cast(solution.at("status_message")); + const auto& status = solution.status_message; ASSERT_TRUE(status == "OptimalSolutionFound" || status == "AcceptableSolutionFound"); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); // size: horizon + 1 - auto U_sol = std::any_cast>(solution.at("control_trajectory")); // size: horizon - auto t_sol = std::any_cast>(solution.at("time_points")); // size: horizon + 1 + const auto& X_sol = solution.state_trajectory; // size: horizon + 1 + const auto& U_sol = solution.control_trajectory; // size: horizon + const auto& t_sol = solution.time_points; // size: horizon + 1 // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -456,10 +456,10 @@ TEST(CLDDPTest, SolveCar) cddp::CDDPSolution solution = cddp_solver.solve("CLDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -470,9 +470,9 @@ TEST(CLDDPTest, SolveCar) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -521,10 +521,10 @@ TEST(CLDDPTest, SolveCar) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -715,10 +715,10 @@ TEST(CLDDPTest, SolveQuadrotor) cddp::CDDPSolution solution = cddp_solver.solve("CLDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Quadrotor Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -728,9 +728,9 @@ TEST(CLDDPTest, SolveQuadrotor) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -798,10 +798,10 @@ TEST(CLDDPTest, SolveQuadrotor) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; diff --git a/tests/cddp_core/test_ipddp_solver.cpp b/tests/cddp_core/test_ipddp_solver.cpp index 4f7b8d88..7e9b99e6 100644 --- a/tests/cddp_core/test_ipddp_solver.cpp +++ b/tests/cddp_core/test_ipddp_solver.cpp @@ -123,10 +123,10 @@ TEST(IPDDPTest, SolvePendulum) cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -136,9 +136,9 @@ TEST(IPDDPTest, SolvePendulum) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -191,10 +191,10 @@ TEST(IPDDPTest, SolvePendulum) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -294,13 +294,13 @@ TEST(IPDDPTest, SolveUnicycle) { cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // cddp::CDDPSolution solution = cddp_solver.solveIPDDP(); - auto status = std::any_cast(solution.at("status_message")); + const auto& status = solution.status_message; ASSERT_TRUE(status == "OptimalSolutionFound" || status == "AcceptableSolutionFound"); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); // size: horizon + 1 - auto U_sol = std::any_cast>(solution.at("control_trajectory")); // size: horizon - auto t_sol = std::any_cast>(solution.at("time_points")); // size: horizon + 1 + const auto& X_sol = solution.state_trajectory; // size: horizon + 1 + const auto& U_sol = solution.control_trajectory; // size: horizon + const auto& t_sol = solution.time_points; // size: horizon + 1 } namespace cddp @@ -450,10 +450,10 @@ TEST(IPDDPTest, SolveCar) cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -464,9 +464,9 @@ TEST(IPDDPTest, SolveCar) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -515,10 +515,10 @@ TEST(IPDDPTest, SolveCar) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -710,10 +710,10 @@ TEST(IPDDPTest, SolveQuadrotor) cddp::CDDPSolution solution = cddp_solver.solve("IPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Quadrotor Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -723,9 +723,9 @@ TEST(IPDDPTest, SolveQuadrotor) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -793,10 +793,10 @@ TEST(IPDDPTest, SolveQuadrotor) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; diff --git a/tests/cddp_core/test_logddp_solver.cpp b/tests/cddp_core/test_logddp_solver.cpp index 274523e9..49728d5b 100644 --- a/tests/cddp_core/test_logddp_solver.cpp +++ b/tests/cddp_core/test_logddp_solver.cpp @@ -123,10 +123,10 @@ TEST(LogDDPTest, SolvePendulum) cddp::CDDPSolution solution = cddp_solver.solve("LogDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -136,9 +136,9 @@ TEST(LogDDPTest, SolvePendulum) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -192,10 +192,10 @@ TEST(LogDDPTest, SolvePendulum) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -295,13 +295,13 @@ TEST(LogDDPTest, SolveUnicycle) { cddp::CDDPSolution solution = cddp_solver.solve("LogDDP"); // cddp::CDDPSolution solution = cddp_solver.solveLogDDP(); - auto status = std::any_cast(solution.at("status_message")); + const auto& status = solution.status_message; ASSERT_TRUE(status == "OptimalSolutionFound" || status == "AcceptableSolutionFound"); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); // size: horizon + 1 - auto U_sol = std::any_cast>(solution.at("control_trajectory")); // size: horizon - auto t_sol = std::any_cast>(solution.at("time_points")); // size: horizon + 1 + const auto& X_sol = solution.state_trajectory; // size: horizon + 1 + const auto& U_sol = solution.control_trajectory; // size: horizon + const auto& t_sol = solution.time_points; // size: horizon + 1 } namespace cddp @@ -455,10 +455,10 @@ TEST(LogDDPTest, SolveCar) cddp::CDDPSolution solution = cddp_solver.solve("LogDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -469,9 +469,9 @@ TEST(LogDDPTest, SolveCar) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -520,10 +520,10 @@ TEST(LogDDPTest, SolveCar) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -720,10 +720,10 @@ TEST(LogDDPTest, SolveQuadrotor) cddp::CDDPSolution solution = cddp_solver.solve("LogDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Quadrotor Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -733,9 +733,9 @@ TEST(LogDDPTest, SolveQuadrotor) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -803,10 +803,10 @@ TEST(LogDDPTest, SolveQuadrotor) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; diff --git a/tests/cddp_core/test_msipddp_solver.cpp b/tests/cddp_core/test_msipddp_solver.cpp index 403073e7..bb2cf607 100644 --- a/tests/cddp_core/test_msipddp_solver.cpp +++ b/tests/cddp_core/test_msipddp_solver.cpp @@ -123,10 +123,10 @@ TEST(MSIPDDPTest, SolvePendulum) cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -136,9 +136,9 @@ TEST(MSIPDDPTest, SolvePendulum) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -191,10 +191,10 @@ TEST(MSIPDDPTest, SolvePendulum) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -294,13 +294,13 @@ TEST(MSIPDDPTest, SolveUnicycle) { cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); // cddp::CDDPSolution solution = cddp_solver.solveMSIPDDP(); - auto status = std::any_cast(solution.at("status_message")); + const auto& status = solution.status_message; ASSERT_TRUE(status == "OptimalSolutionFound" || status == "AcceptableSolutionFound"); // Extract solution - auto X_sol = std::any_cast>(solution.at("state_trajectory")); // size: horizon + 1 - auto U_sol = std::any_cast>(solution.at("control_trajectory")); // size: horizon - auto t_sol = std::any_cast>(solution.at("time_points")); // size: horizon + 1 + const auto& X_sol = solution.state_trajectory; // size: horizon + 1 + const auto& U_sol = solution.control_trajectory; // size: horizon + const auto& t_sol = solution.time_points; // size: horizon + 1 } namespace cddp @@ -450,10 +450,10 @@ TEST(MSIPDDPTest, SolveCar) cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -464,9 +464,9 @@ TEST(MSIPDDPTest, SolveCar) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -515,10 +515,10 @@ TEST(MSIPDDPTest, SolveCar) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; @@ -710,10 +710,10 @@ TEST(MSIPDDPTest, SolveQuadrotor) cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP"); // Check convergence - auto status_message = std::any_cast(solution.at("status_message")); - auto iterations_completed = std::any_cast(solution.at("iterations_completed")); - auto solve_time_ms = std::any_cast(solution.at("solve_time_ms")); - auto final_objective = std::any_cast(solution.at("final_objective")); + const auto& status_message = solution.status_message; + auto iterations_completed = solution.iterations_completed; + auto solve_time_ms = solution.solve_time_ms; + auto final_objective = solution.final_objective; std::cout << "\n=== Quadrotor Convergence Analysis ===" << std::endl; std::cout << "Status: " << status_message << std::endl; @@ -723,9 +723,9 @@ TEST(MSIPDDPTest, SolveQuadrotor) std::cout << "Final cost: " << final_objective << std::endl; // Extract trajectories - auto X_sol = std::any_cast>(solution.at("state_trajectory")); - auto U_sol = std::any_cast>(solution.at("control_trajectory")); - auto t_sol = std::any_cast>(solution.at("time_points")); + const auto& X_sol = solution.state_trajectory; + const auto& U_sol = solution.control_trajectory; + const auto& t_sol = solution.time_points; // Print final state Eigen::VectorXd final_state = X_sol.back(); @@ -793,10 +793,10 @@ TEST(MSIPDDPTest, SolveQuadrotor) auto warm_duration = std::chrono::duration_cast(end_time - start_time); // Extract warm start results - auto warm_status = std::any_cast(warm_solution.at("status_message")); - auto warm_iterations = std::any_cast(warm_solution.at("iterations_completed")); - auto warm_solve_time = std::any_cast(warm_solution.at("solve_time_ms")); - auto warm_final_cost = std::any_cast(warm_solution.at("final_objective")); + const auto& warm_status = warm_solution.status_message; + auto warm_iterations = warm_solution.iterations_completed; + auto warm_solve_time = warm_solution.solve_time_ms; + auto warm_final_cost = warm_solution.final_objective; std::cout << "Warm start status: " << warm_status << std::endl; std::cout << "Warm start iterations: " << warm_iterations << std::endl; From 8efb1d8acf2a6c0e6a4a1d373cac378cbae69be6 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 12:44:16 -0400 Subject: [PATCH 3/7] Add example utilities and simplify 29 example files Create cddp_example_utils.hpp with extractComponent, makeTimeVector, ensurePlotDir, and makeInitialTrajectory helpers. Apply across all examples to reduce boilerplate data extraction and directory setup. --- examples/cddp_acrobot.cpp | 15 ++--- examples/cddp_bicycle.cpp | 26 +++----- examples/cddp_car.cpp | 14 ++--- examples/cddp_car_ipddp.cpp | 13 ++-- examples/cddp_cartpole.cpp | 49 +++------------ examples/cddp_forklift_ipddp.cpp | 13 ++-- examples/cddp_hcw.cpp | 5 +- examples/cddp_lti_system.cpp | 5 +- examples/cddp_manipulator.cpp | 5 +- examples/cddp_pendulum.cpp | 29 +++------ examples/cddp_quadrotor_circle.cpp | 1 + ...cddp_quadrotor_figure_eight_horizontal.cpp | 6 +- ...quadrotor_figure_eight_horizontal_safe.cpp | 6 +- .../cddp_quadrotor_figure_eight_vertical.cpp | 6 +- examples/cddp_quadrotor_point.cpp | 6 +- examples/cddp_spacecraft_linear_docking.cpp | 1 + examples/cddp_spacecraft_linear_rpo.cpp | 1 + examples/cddp_spacecraft_nonlinear_rpo.cpp | 1 + examples/cddp_spacecraft_roe_rpo.cpp | 1 + examples/cddp_spacecraft_rpo.cpp | 1 + examples/cddp_spacecraft_rpo_fuel.cpp | 1 + examples/cddp_spacecraft_rpo_mc.cpp | 1 + examples/cddp_unicycle.cpp | 21 +++---- examples/cddp_unicycle_mpc.cpp | 5 +- examples/cddp_unicycle_safe.cpp | 5 +- examples/cddp_unicycle_safe_ipddp.cpp | 7 +-- examples/cddp_unicycle_safe_ipddp_v2.cpp | 11 +--- examples/cddp_unicycle_safe_ipddp_v3.cpp | 11 +--- examples/mpc_hcw.cpp | 10 +-- include/cddp-cpp/cddp_example_utils.hpp | 61 +++++++++++++++++++ 30 files changed, 149 insertions(+), 188 deletions(-) create mode 100644 include/cddp-cpp/cddp_example_utils.hpp diff --git a/examples/cddp_acrobot.cpp b/examples/cddp_acrobot.cpp index a2d64f95..b1f00a75 100644 --- a/examples/cddp_acrobot.cpp +++ b/examples/cddp_acrobot.cpp @@ -22,6 +22,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "dynamics_model/acrobot.hpp" #include "matplot/matplot.h" @@ -111,18 +112,12 @@ int main() { // Create plot directory const std::string plotDirectory = "../results/acrobot"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Extract solution data for animation - std::vector time_arr, theta1_arr, theta2_arr; - - for (size_t i = 0; i < X_sol.size(); ++i) { - time_arr.push_back(t_sol[i]); - theta1_arr.push_back(X_sol[i](0)); - theta2_arr.push_back(X_sol[i](1)); - } + const auto& time_arr = t_sol; + auto theta1_arr = cddp::example::extractComponent(X_sol, 0); + auto theta2_arr = cddp::example::extractComponent(X_sol, 1); // --- Animation --- auto fig = figure(); diff --git a/examples/cddp_bicycle.cpp b/examples/cddp_bicycle.cpp index dc750ff1..5d2c6a59 100644 --- a/examples/cddp_bicycle.cpp +++ b/examples/cddp_bicycle.cpp @@ -5,6 +5,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" namespace fs = std::filesystem; @@ -72,31 +73,20 @@ int main() // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void) std::system("mkdir -p frames"); // Extract trajectory data - std::vector x_arr, y_arr, theta_arr, v_arr; - for (const auto &x : X_sol) - { - x_arr.push_back(x(0)); - y_arr.push_back(x(1)); - theta_arr.push_back(x(2)); - v_arr.push_back(x(3)); - } + auto x_arr = cddp::example::extractComponent(X_sol, 0); + auto y_arr = cddp::example::extractComponent(X_sol, 1); + auto theta_arr = cddp::example::extractComponent(X_sol, 2); + auto v_arr = cddp::example::extractComponent(X_sol, 3); // Extract control inputs - std::vector acc_arr, steering_arr; - for (const auto &u : U_sol) - { - acc_arr.push_back(u(0)); - steering_arr.push_back(u(1)); - } + auto acc_arr = cddp::example::extractComponent(U_sol, 0); + auto steering_arr = cddp::example::extractComponent(U_sol, 1); // ----------------------------- // Plot states and controls diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp index 558796d6..05b7151a 100644 --- a/examples/cddp_car.cpp +++ b/examples/cddp_car.cpp @@ -19,6 +19,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -256,12 +257,8 @@ int main() const auto& t_sol = solution.time_points; // Prepare trajectory data - std::vector x_hist, y_hist; - for (const auto &x : X_sol) - { - x_hist.push_back(x(0)); - y_hist.push_back(x(1)); - } + auto x_hist = cddp::example::extractComponent(X_sol, 0); + auto y_hist = cddp::example::extractComponent(X_sol, 1); // Car dimensions. double car_length = 2.1; @@ -275,10 +272,7 @@ int main() // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void) std::system("mkdir -p frames"); diff --git a/examples/cddp_car_ipddp.cpp b/examples/cddp_car_ipddp.cpp index 4551d483..044cbef5 100644 --- a/examples/cddp_car_ipddp.cpp +++ b/examples/cddp_car_ipddp.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -209,11 +210,8 @@ int main() { const auto& t_sol = solution.time_points; // Prepare trajectory data for plotting - std::vector x_hist, y_hist; - for (const auto& state : X_sol) { - x_hist.push_back(state(0)); - y_hist.push_back(state(1)); - } + auto x_hist = cddp::example::extractComponent(X_sol, 0); + auto y_hist = cddp::example::extractComponent(X_sol, 1); // Car dimensions. double car_length = 2.1; double car_width = 0.9; @@ -226,10 +224,7 @@ int main() { // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void) std::system("mkdir -p frames"); diff --git a/examples/cddp_cartpole.cpp b/examples/cddp_cartpole.cpp index d84c2b93..b459fccb 100644 --- a/examples/cddp_cartpole.cpp +++ b/examples/cddp_cartpole.cpp @@ -20,6 +20,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" #include @@ -99,30 +100,7 @@ int main() { // std::make_unique( control_lower_bound, control_upper_bound)); // Initial trajectory. - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - // Generate initial trajectory by constant initial state - for (int i = 0; i < horizon + 1; ++i) { - X[i] = initial_state; - } - // // Generate initial trajectory by random - // for (int i = 0; i < horizon + 1; ++i) { - // std::random_device rd; - // std::mt19937 gen(rd()); - // std::uniform_real_distribution dist(-0.025, 0.025); - // X[i] = initial_state; - // X[i](0) += dist(gen); - // X[i](1) += dist(gen); - // X[i](2) += dist(gen); - // X[i](3) += dist(gen); - // } - // for (int i = 0; i < horizon; ++i) { - // std::random_device rd; - // std::mt19937 gen(rd()); - // std::uniform_real_distribution dist(-0.005, 0.005); - // U[i] = Eigen::VectorXd::Zero(control_dim); - // U[i](0) += dist(gen); - // } + auto [X, U] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); cddp_solver.setInitialTrajectory(X, U); // Solve. @@ -133,26 +111,19 @@ int main() { // Create plot directory. const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void) std::system("mkdir -p frames"); // Extract solution data. - std::vector x_arr, x_dot_arr, theta_arr, theta_dot_arr, force_arr, time_arr, time_arr2; - for (size_t i = 0; i < X_sol.size(); ++i) { - time_arr.push_back(t_sol[i]); - x_arr.push_back(X_sol[i](0)); - theta_arr.push_back(X_sol[i](1)); - x_dot_arr.push_back(X_sol[i](2)); - theta_dot_arr.push_back(X_sol[i](3)); - } - for (size_t i = 0; i < U_sol.size(); ++i) { - force_arr.push_back(U_sol[i](0)); - time_arr2.push_back(t_sol[i]); - } + auto x_arr = cddp::example::extractComponent(X_sol, 0); + auto theta_arr = cddp::example::extractComponent(X_sol, 1); + auto x_dot_arr = cddp::example::extractComponent(X_sol, 2); + auto theta_dot_arr = cddp::example::extractComponent(X_sol, 3); + auto force_arr = cddp::example::extractComponent(U_sol, 0); + const auto& time_arr = t_sol; + std::vector time_arr2(t_sol.begin(), t_sol.begin() + U_sol.size()); // --- Plot static results (2x2 plots for state trajectories) --- auto fig1 = figure(); diff --git a/examples/cddp_forklift_ipddp.cpp b/examples/cddp_forklift_ipddp.cpp index fb0790d6..17040836 100644 --- a/examples/cddp_forklift_ipddp.cpp +++ b/examples/cddp_forklift_ipddp.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "dynamics_model/forklift.hpp" #include "matplot/matplot.h" #include @@ -251,11 +252,8 @@ int main() { const auto& t_sol = solution.time_points; // Prepare trajectory data for plotting - std::vector x_hist, y_hist; - for (const auto& state : X_sol) { - x_hist.push_back(state(0)); - y_hist.push_back(state(1)); - } + auto x_hist = cddp::example::extractComponent(X_sol, 0); + auto y_hist = cddp::example::extractComponent(X_sol, 1); // Forklift dimensions double forklift_length = 2.5; // Overall length @@ -269,10 +267,7 @@ int main() { // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Animation loop: update plot for each time step and save frame for (size_t i = 0; i < X_sol.size(); ++i) diff --git a/examples/cddp_hcw.cpp b/examples/cddp_hcw.cpp index 9420df5c..9c4883c6 100644 --- a/examples/cddp_hcw.cpp +++ b/examples/cddp_hcw.cpp @@ -26,6 +26,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -159,9 +160,7 @@ int main() { // Create plot directory const std::string plotDirectory = "../results/tests"; - if (!std::filesystem::exists(plotDirectory)) { - std::filesystem::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Extract state data arrays std::vector x_arr, y_arr, z_arr, vx_arr, vy_arr, vz_arr, time_arr; diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp index 0d704109..f2d3ec9d 100644 --- a/examples/cddp_lti_system.cpp +++ b/examples/cddp_lti_system.cpp @@ -3,6 +3,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -157,9 +158,7 @@ int main() { // Create directory for plots if it doesn't exist const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Plot state trajectories using the matplot API plotStateTrajectories(X_sol, plotDirectory); diff --git a/examples/cddp_manipulator.cpp b/examples/cddp_manipulator.cpp index af7acf2e..24278b5f 100644 --- a/examples/cddp_manipulator.cpp +++ b/examples/cddp_manipulator.cpp @@ -22,6 +22,7 @@ #include // for sleep_for #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -161,9 +162,7 @@ int main() { // Create a directory for plots if it doesn't exist const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); save(fig1, plotDirectory + "/manipulator_cddp_results.png"); // Create a new figure for animation diff --git a/examples/cddp_pendulum.cpp b/examples/cddp_pendulum.cpp index 371d657d..19654f63 100644 --- a/examples/cddp_pendulum.cpp +++ b/examples/cddp_pendulum.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -98,11 +99,7 @@ int main() { std::make_unique(control_lower_bound, control_upper_bound)); // Set initial trajectory for the solver - std::vector X(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U(horizon, Eigen::VectorXd::Zero(control_dim)); - for (int i = 0; i < horizon + 1; ++i) { - X[i] = initial_state; - } + auto [X, U] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); cddp_solver.setInitialTrajectory(X, U); // Solve the optimal control problem @@ -112,26 +109,16 @@ int main() { // Create plot directory if it doesn't exist const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // -------------------- Data Extraction -------------------- - std::vector theta_arr, theta_dot_arr, torque_arr; - for (const auto& x : X_sol) { - theta_arr.push_back(x(0)); - theta_dot_arr.push_back(x(1)); - } - for (const auto& u : U_sol) { - torque_arr.push_back(u(0)); - } + auto theta_arr = cddp::example::extractComponent(X_sol, 0); + auto theta_dot_arr = cddp::example::extractComponent(X_sol, 1); + auto torque_arr = cddp::example::extractComponent(U_sol, 0); // Build time vectors (state has horizon+1 points; control has horizon points) - std::vector time_state, time_control; - for (size_t i = 0; i < theta_arr.size(); ++i) - time_state.push_back(i * timestep); - for (size_t i = 0; i < torque_arr.size(); ++i) - time_control.push_back(i * timestep); + auto time_state = cddp::example::makeTimeVector(X_sol.size(), timestep); + auto time_control = cddp::example::makeTimeVector(U_sol.size(), timestep); // -------------------- Static Plot -------------------- auto fig1 = figure(true); diff --git a/examples/cddp_quadrotor_circle.cpp b/examples/cddp_quadrotor_circle.cpp index bfd0d8de..22f7ec0b 100644 --- a/examples/cddp_quadrotor_circle.cpp +++ b/examples/cddp_quadrotor_circle.cpp @@ -20,6 +20,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; diff --git a/examples/cddp_quadrotor_figure_eight_horizontal.cpp b/examples/cddp_quadrotor_figure_eight_horizontal.cpp index 1e4a97b5..04c0771a 100644 --- a/examples/cddp_quadrotor_figure_eight_horizontal.cpp +++ b/examples/cddp_quadrotor_figure_eight_horizontal.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -246,10 +247,7 @@ int main() // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void)std::system("mkdir -p frames"); diff --git a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp b/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp index 50863e33..f26ffee5 100644 --- a/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp +++ b/examples/cddp_quadrotor_figure_eight_horizontal_safe.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -275,10 +276,7 @@ int main() // Create directory for saving plots const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void)std::system("mkdir -p frames"); diff --git a/examples/cddp_quadrotor_figure_eight_vertical.cpp b/examples/cddp_quadrotor_figure_eight_vertical.cpp index 7b19dfd8..ba288c05 100644 --- a/examples/cddp_quadrotor_figure_eight_vertical.cpp +++ b/examples/cddp_quadrotor_figure_eight_vertical.cpp @@ -21,6 +21,7 @@ #include #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -247,10 +248,7 @@ int main() // Create plot directory if it doesn't exist const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void)std::system("mkdir -p frames"); diff --git a/examples/cddp_quadrotor_point.cpp b/examples/cddp_quadrotor_point.cpp index 9bb29593..42fb07dc 100644 --- a/examples/cddp_quadrotor_point.cpp +++ b/examples/cddp_quadrotor_point.cpp @@ -20,6 +20,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -208,10 +209,7 @@ int main() // Create plot directory if it doesn't exist const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) - { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Create a directory for frame images. (void)std::system("mkdir -p frames"); diff --git a/examples/cddp_spacecraft_linear_docking.cpp b/examples/cddp_spacecraft_linear_docking.cpp index 33ef3a0d..85addef8 100644 --- a/examples/cddp_spacecraft_linear_docking.cpp +++ b/examples/cddp_spacecraft_linear_docking.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" namespace fs = std::filesystem; using namespace cddp; diff --git a/examples/cddp_spacecraft_linear_rpo.cpp b/examples/cddp_spacecraft_linear_rpo.cpp index 85886d14..5cb94a60 100644 --- a/examples/cddp_spacecraft_linear_rpo.cpp +++ b/examples/cddp_spacecraft_linear_rpo.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" namespace cddp { diff --git a/examples/cddp_spacecraft_nonlinear_rpo.cpp b/examples/cddp_spacecraft_nonlinear_rpo.cpp index e127f8ae..e727519a 100644 --- a/examples/cddp_spacecraft_nonlinear_rpo.cpp +++ b/examples/cddp_spacecraft_nonlinear_rpo.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" namespace cddp { diff --git a/examples/cddp_spacecraft_roe_rpo.cpp b/examples/cddp_spacecraft_roe_rpo.cpp index 7420a895..e35de013 100644 --- a/examples/cddp_spacecraft_roe_rpo.cpp +++ b/examples/cddp_spacecraft_roe_rpo.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "dynamics_model/spacecraft_roe.hpp" // Added for SpacecraftROE namespace plt = matplot; diff --git a/examples/cddp_spacecraft_rpo.cpp b/examples/cddp_spacecraft_rpo.cpp index 3c95dd12..c2ab319c 100644 --- a/examples/cddp_spacecraft_rpo.cpp +++ b/examples/cddp_spacecraft_rpo.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" namespace cddp { diff --git a/examples/cddp_spacecraft_rpo_fuel.cpp b/examples/cddp_spacecraft_rpo_fuel.cpp index b769dbd8..23fb8ef0 100644 --- a/examples/cddp_spacecraft_rpo_fuel.cpp +++ b/examples/cddp_spacecraft_rpo_fuel.cpp @@ -24,6 +24,7 @@ #include // For std::sqrt #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "cddp_core/cddp_core.hpp" #include "cddp_core/objective.hpp" #include "dynamics_model/spacecraft_linear_fuel.hpp" diff --git a/examples/cddp_spacecraft_rpo_mc.cpp b/examples/cddp_spacecraft_rpo_mc.cpp index 23454487..b8ef0d98 100644 --- a/examples/cddp_spacecraft_rpo_mc.cpp +++ b/examples/cddp_spacecraft_rpo_mc.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" namespace cddp { diff --git a/examples/cddp_unicycle.cpp b/examples/cddp_unicycle.cpp index 1818fc05..df5969a2 100644 --- a/examples/cddp_unicycle.cpp +++ b/examples/cddp_unicycle.cpp @@ -18,6 +18,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -88,24 +89,16 @@ int main() { // Create directory for saving plot (if it doesn't exist) const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directory(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Plot the solution (x-y plane) - std::vector x_arr, y_arr, theta_arr; - for (const auto& x : X_sol) { - x_arr.push_back(x(0)); - y_arr.push_back(x(1)); - theta_arr.push_back(x(2)); - } + auto x_arr = cddp::example::extractComponent(X_sol, 0); + auto y_arr = cddp::example::extractComponent(X_sol, 1); + auto theta_arr = cddp::example::extractComponent(X_sol, 2); // Plot the solution (control inputs) - std::vector v_arr, omega_arr; - for (const auto& u : U_sol) { - v_arr.push_back(u(0)); - omega_arr.push_back(u(1)); - } + auto v_arr = cddp::example::extractComponent(U_sol, 0); + auto omega_arr = cddp::example::extractComponent(U_sol, 1); // ----------------------------- // Plot states and controls diff --git a/examples/cddp_unicycle_mpc.cpp b/examples/cddp_unicycle_mpc.cpp index 06117fa2..e798d161 100644 --- a/examples/cddp_unicycle_mpc.cpp +++ b/examples/cddp_unicycle_mpc.cpp @@ -24,6 +24,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" namespace fs = std::filesystem; @@ -279,9 +280,7 @@ int main() // Save and show plot const std::string plotDirectory = "../results/examples"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); save(plotDirectory + "/unicycle_mpc_cbf.png"); std::cout << "Saved plot to " << plotDirectory << "/unicycle_mpc_cbf.png" << std::endl; show(); diff --git a/examples/cddp_unicycle_safe.cpp b/examples/cddp_unicycle_safe.cpp index 12c89162..a8f24bc2 100644 --- a/examples/cddp_unicycle_safe.cpp +++ b/examples/cddp_unicycle_safe.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" // Include matplot #include "matplot/matplot.h" @@ -206,9 +207,7 @@ int main() { // Create directory for saving (if not existing) std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // Save figure f1->draw(); diff --git a/examples/cddp_unicycle_safe_ipddp.cpp b/examples/cddp_unicycle_safe_ipddp.cpp index 17602537..7afcc951 100644 --- a/examples/cddp_unicycle_safe_ipddp.cpp +++ b/examples/cddp_unicycle_safe_ipddp.cpp @@ -23,7 +23,8 @@ #include #include -#include "cddp.hpp" +#include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" namespace fs = std::filesystem; @@ -86,9 +87,7 @@ int main() { // Create a directory for saving plots (if it doesn't exist) const std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); // -------------------------- // 2. Solve - NO Ball constraint diff --git a/examples/cddp_unicycle_safe_ipddp_v2.cpp b/examples/cddp_unicycle_safe_ipddp_v2.cpp index 7191ad3d..240fd970 100644 --- a/examples/cddp_unicycle_safe_ipddp_v2.cpp +++ b/examples/cddp_unicycle_safe_ipddp_v2.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" // Include matplot #include "matplot/matplot.h" @@ -109,11 +110,7 @@ int main() { std::make_unique(radius2, center2)); // Initial trajectory guess - std::vector X_sol(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd::Zero(control_dim)); - for (int i = 0; i < horizon + 1; ++i) { - X_sol[i] = initial_state; - } + auto [X_sol, U_sol] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); cddp_solver.setInitialTrajectory(X_sol, U_sol); // Solve @@ -184,9 +181,7 @@ int main() { // 5. Save Plot // ------------------------------------------------- std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); f1->draw(); f1->save(plotDirectory + "/trajectory_comparison_ipddp_v2_matplot.png"); diff --git a/examples/cddp_unicycle_safe_ipddp_v3.cpp b/examples/cddp_unicycle_safe_ipddp_v3.cpp index cdc908db..2eb6f6d4 100644 --- a/examples/cddp_unicycle_safe_ipddp_v3.cpp +++ b/examples/cddp_unicycle_safe_ipddp_v3.cpp @@ -22,6 +22,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" // Include matplot #include "matplot/matplot.h" @@ -118,11 +119,7 @@ int main() { std::make_unique(A, b)); // Initial trajectory guess - std::vector X_sol(horizon + 1, Eigen::VectorXd::Zero(state_dim)); - std::vector U_sol(horizon, Eigen::VectorXd::Zero(control_dim)); - for (int i = 0; i < horizon + 1; ++i) { - X_sol[i] = initial_state; - } + auto [X_sol, U_sol] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim); cddp_solver.setInitialTrajectory(X_sol, U_sol); // Solve @@ -211,9 +208,7 @@ int main() { // 5. Save Plot // ------------------------------------------------- std::string plotDirectory = "../results/tests"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); f1->draw(); f1->save(plotDirectory + "/trajectory_comparison_ipddp_v3_linear_matplot.png"); diff --git a/examples/mpc_hcw.cpp b/examples/mpc_hcw.cpp index af757f2c..2a49e457 100644 --- a/examples/mpc_hcw.cpp +++ b/examples/mpc_hcw.cpp @@ -21,6 +21,7 @@ #include #include "cddp.hpp" +#include "cddp_example_utils.hpp" #include "matplot/matplot.h" using namespace matplot; @@ -320,9 +321,7 @@ int main() { // Create results directory const std::string plotDirectory = "../results/simulations"; - if (!fs::exists(plotDirectory)) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); std::string figPath = plotDirectory + "/hcw_mpc_cddp_xaxis_trajectories.png"; save(figPath); // show(); @@ -375,10 +374,7 @@ int main() { } // Create results directory const std::string plotDirectory = "../results/simulations"; - if (!fs::exists(plotDirectory - )) { - fs::create_directories(plotDirectory); - } + cddp::example::ensurePlotDir(plotDirectory); std::string figPath = plotDirectory + "/hcw_mpc_cddp_controls.png"; save(figPath); // show(); diff --git a/include/cddp-cpp/cddp_example_utils.hpp b/include/cddp-cpp/cddp_example_utils.hpp new file mode 100644 index 00000000..e3d24917 --- /dev/null +++ b/include/cddp-cpp/cddp_example_utils.hpp @@ -0,0 +1,61 @@ +/* + Copyright 2025 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_EXAMPLE_UTILS_HPP +#define CDDP_EXAMPLE_UTILS_HPP + +#include +#include +#include + +namespace cddp::example { + +/// Extract a single scalar component from a trajectory of vectors. +inline std::vector +extractComponent(const std::vector &traj, int index) { + std::vector result; + result.reserve(traj.size()); + for (const auto &v : traj) + result.push_back(v(index)); + return result; +} + +/// Build a uniform time vector: [0, dt, 2*dt, ..., (count-1)*dt]. +inline std::vector makeTimeVector(int count, double dt) { + std::vector t(count); + for (int i = 0; i < count; ++i) + t[i] = i * dt; + return t; +} + +/// Ensure a directory exists (creates parents if needed). +inline void ensurePlotDir(const std::string &dir) { + if (!std::filesystem::exists(dir)) + std::filesystem::create_directories(dir); +} + +/// Create a zero-control initial trajectory with the initial state replicated. +inline std::pair, std::vector> +makeInitialTrajectory(const Eigen::VectorXd &initial_state, int horizon, + int control_dim) { + return {std::vector(horizon + 1, initial_state), + std::vector(horizon, + Eigen::VectorXd::Zero(control_dim))}; +} + +} // namespace cddp::example + +#endif // CDDP_EXAMPLE_UTILS_HPP From 59ec6acc7c53f688c63a7f53ed85852c5940d526 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 13:41:13 -0400 Subject: [PATCH 4/7] Fix issues found in PR review - Skip convergence check after failed forward pass (prevents false convergence when dJ/dL are zero, which triggered LogDDP's abs check) - Log unconditionally when ALL parallel forward pass threads fail - Add checkEarlyConvergence hook so CLDDP checks inf_du before forward pass - Record iteration history on early convergence path - Fix BoxConstraint::computeViolation double-subtraction of ip_upper_bound_ - Add scale_factor_ to BoxConstraint control Jacobian for consistency - Unify CDDPSolution::History and IterationHistory into single type - Clean up solve() loop: remove continue, single printIteration call site --- include/cddp-cpp/cddp_core/cddp_core.hpp | 15 ++++ .../cddp-cpp/cddp_core/cddp_solver_base.hpp | 28 +++---- include/cddp-cpp/cddp_core/clddp_solver.hpp | 2 + include/cddp-cpp/cddp_core/constraint.hpp | 6 +- src/cddp_core/cddp_solver_base.cpp | 81 ++++++++----------- src/cddp_core/clddp_solver.cpp | 9 +++ 6 files changed, 75 insertions(+), 66 deletions(-) diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index 9a0bdaa1..adbd884d 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -85,6 +85,21 @@ struct CDDPSolution { std::vector complementary_infeasibility; std::vector barrier_mu; std::vector regularization; + + void reserve(size_t n) { + objective.reserve(n); merit_function.reserve(n); + step_length_primal.reserve(n); step_length_dual.reserve(n); + dual_infeasibility.reserve(n); primal_infeasibility.reserve(n); + complementary_infeasibility.reserve(n); barrier_mu.reserve(n); + regularization.reserve(n); + } + void clear() { + objective.clear(); merit_function.clear(); + step_length_primal.clear(); step_length_dual.clear(); + dual_infeasibility.clear(); primal_infeasibility.clear(); + complementary_infeasibility.clear(); barrier_mu.clear(); + regularization.clear(); + } } history; }; diff --git a/include/cddp-cpp/cddp_core/cddp_solver_base.hpp b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp index 722ce00d..a4173bfa 100644 --- a/include/cddp-cpp/cddp_core/cddp_solver_base.hpp +++ b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp @@ -67,6 +67,17 @@ class CDDPSolverBase : public ISolverAlgorithm { */ virtual bool backwardPass(CDDP &context) = 0; + /** + * @brief Optional early convergence check after backward pass, before + * forward pass. Override to check dual infeasibility without running + * an unnecessary forward pass (e.g., CLDDP checks inf_du here). + * @return True if converged. + */ + virtual bool checkEarlyConvergence(CDDP &context, int iter, + std::string &reason) { + return false; + } + /** * @brief Perform a single forward pass trial with given step size. */ @@ -157,21 +168,8 @@ class CDDPSolverBase : public ISolverAlgorithm { */ void computeCost(CDDP &context); - // === History tracking === - struct IterationHistory { - std::vector objective; - std::vector merit_function; - std::vector step_length_primal; - std::vector step_length_dual; - std::vector dual_infeasibility; - std::vector primal_infeasibility; - std::vector complementary_infeasibility; - std::vector barrier_mu; - std::vector regularization; - - void reserve(size_t n); - void clear(); - } history_; + // History tracking (reuses CDDPSolution::History to avoid type duplication) + CDDPSolution::History history_; }; } // namespace cddp diff --git a/include/cddp-cpp/cddp_core/clddp_solver.hpp b/include/cddp-cpp/cddp_core/clddp_solver.hpp index 2b25f552..12233db6 100644 --- a/include/cddp-cpp/cddp_core/clddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/clddp_solver.hpp @@ -37,6 +37,8 @@ class CLDDPSolver : public CDDPSolverBase { protected: bool backwardPass(CDDP &context) override; + bool checkEarlyConvergence(CDDP &context, int iter, + std::string &reason) override; ForwardPassResult forwardPass(CDDP &context, double alpha) override; bool checkConvergence(CDDP &context, double dJ, double dL, int iter, std::string &reason) override; diff --git a/include/cddp-cpp/cddp_core/constraint.hpp b/include/cddp-cpp/cddp_core/constraint.hpp index ec4e8319..14ed3efa 100644 --- a/include/cddp-cpp/cddp_core/constraint.hpp +++ b/include/cddp-cpp/cddp_core/constraint.hpp @@ -210,9 +210,9 @@ namespace cddp { Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(dim_, control.size()); jac.topLeftCorner(control.size(), control.size()) = - -Eigen::MatrixXd::Identity(control.size(), control.size()); + -Eigen::MatrixXd::Identity(control.size(), control.size()) * scale_factor_; jac.bottomRightCorner(control.size(), control.size()) = - Eigen::MatrixXd::Identity(control.size(), control.size()); + Eigen::MatrixXd::Identity(control.size(), control.size()) * scale_factor_; return jac; } } @@ -229,7 +229,7 @@ namespace cddp const Eigen::VectorXd &control, int index = 0) const override { - return computeViolationFromValue(evaluate(state, control, index) - ip_upper_bound_); + return computeViolationFromValue(evaluate(state, control, index)); } double computeViolationFromValue(const Eigen::VectorXd &g) const override diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index 569eca32..c423245b 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -23,32 +23,6 @@ namespace cddp { -// === IterationHistory === - -void CDDPSolverBase::IterationHistory::reserve(size_t n) { - objective.reserve(n); - merit_function.reserve(n); - step_length_primal.reserve(n); - step_length_dual.reserve(n); - dual_infeasibility.reserve(n); - primal_infeasibility.reserve(n); - complementary_infeasibility.reserve(n); - barrier_mu.reserve(n); - regularization.reserve(n); -} - -void CDDPSolverBase::IterationHistory::clear() { - objective.clear(); - merit_function.clear(); - step_length_primal.clear(); - step_length_dual.clear(); - dual_infeasibility.clear(); - primal_infeasibility.clear(); - complementary_infeasibility.clear(); - barrier_mu.clear(); - regularization.clear(); -} - // === Template method: solve() === CDDPSolution CDDPSolverBase::solve(CDDP &context) { @@ -134,31 +108,39 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { if (!backward_ok) break; + // Early convergence check (e.g., CLDDP checks inf_du after backward pass) + if (checkEarlyConvergence(context, iter, termination_reason)) { + converged = true; + if (options.return_iteration_info) { + recordIterationHistory(context); + } + if (options.verbose) { + printIteration(iter, context); + } + break; + } + // Forward pass with line search ForwardPassResult best_result = performForwardPass(context); + bool fp_success = best_result.success; - if (best_result.success) { + if (fp_success) { dJ = context.cost_ - best_result.cost; dL = context.merit_function_ - best_result.merit_function; - applyForwardPassResult(context, best_result); - if (options.return_iteration_info) { recordIterationHistory(context); } - context.decreaseRegularization(); + + // Check convergence (only meaningful after successful forward pass) + converged = checkConvergence(context, dJ, dL, iter, termination_reason); } else { - // Handle failure (default: increase regularization; MSIPDDP: try filter - // restoration first) bool should_break = handleForwardPassFailure(context, termination_reason); if (should_break) break; } - // Check convergence - converged = checkConvergence(context, dJ, dL, iter, termination_reason); - if (options.verbose) { printIteration(iter, context); } @@ -166,8 +148,7 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { if (converged) break; - // Post-iteration update (barrier parameter update, etc.) - postIterationUpdate(context, best_result.success); + postIterationUpdate(context, fp_success); } // Compute final timing @@ -187,18 +168,9 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { solution.feedback_gains = K_u_; solution.final_regularization = context.regularization_; - // Add iteration history if requested + // Move iteration history into solution (same type — no field-by-field copy) if (options.return_iteration_info) { - solution.history.objective = history_.objective; - solution.history.merit_function = history_.merit_function; - solution.history.step_length_primal = history_.step_length_primal; - solution.history.step_length_dual = history_.step_length_dual; - solution.history.dual_infeasibility = history_.dual_infeasibility; - solution.history.primal_infeasibility = history_.primal_infeasibility; - solution.history.complementary_infeasibility = - history_.complementary_infeasibility; - solution.history.barrier_mu = history_.barrier_mu; - solution.history.regularization = history_.regularization; + solution.history = std::move(history_); } // Solver-specific solution fields @@ -245,6 +217,9 @@ void CDDPSolverBase::recordIterationHistory(const CDDP &context) { history_.dual_infeasibility.push_back(context.inf_du_); history_.primal_infeasibility.push_back(context.inf_pr_); history_.complementary_infeasibility.push_back(context.inf_comp_); + // Note: barrier_mu is NOT pushed here. IP solvers override recordIterationHistory + // to push their actual mu_ value. Non-IP solvers (CLDDP) don't use barrier_mu, + // so it stays empty for them. Consumers must check vector sizes. history_.regularization.push_back(context.regularization_); } @@ -289,6 +264,8 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { [this, &context, alpha]() { return forwardPass(context, alpha); })); } + int failed_count = 0; + std::string last_error; for (auto &future : futures) { try { if (future.valid()) { @@ -299,6 +276,8 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { } } } catch (const std::exception &e) { + ++failed_count; + last_error = e.what(); if (options.verbose) { std::cerr << getSolverName() << ": Forward pass thread failed: " << e.what() @@ -306,6 +285,12 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { } } } + if (failed_count > 0 && + failed_count == static_cast(futures.size())) { + std::cerr << getSolverName() + << ": ALL forward pass threads failed. Last error: " + << last_error << std::endl; + } } return best_result; diff --git a/src/cddp_core/clddp_solver.cpp b/src/cddp_core/clddp_solver.cpp index 51176554..1ae342d0 100644 --- a/src/cddp_core/clddp_solver.cpp +++ b/src/cddp_core/clddp_solver.cpp @@ -203,6 +203,15 @@ bool CLDDPSolver::backwardPass(CDDP &context) { return true; } +bool CLDDPSolver::checkEarlyConvergence(CDDP &context, int /*iter*/, + std::string &reason) { + if (context.inf_du_ < context.getOptions().tolerance) { + reason = "OptimalSolutionFound"; + return true; + } + return false; +} + ForwardPassResult CLDDPSolver::forwardPass(CDDP &context, double alpha_pr) { ForwardPassResult result; result.success = false; From eda14e8b7d59f32a0df37d057c7edd9aa0bf67d8 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 15:23:49 -0400 Subject: [PATCH 5/7] Fix crash in cddp_lti_system and cddp_hcw examples Both examples accessed solution.history.objective.back() but history is only populated when return_iteration_info is enabled (default false). Use solution.final_objective instead. Also fix ms-vs-microseconds label. --- examples/cddp_hcw.cpp | 6 ++---- examples/cddp_lti_system.cpp | 6 ++---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/examples/cddp_hcw.cpp b/examples/cddp_hcw.cpp index 9c4883c6..b9a05085 100644 --- a/examples/cddp_hcw.cpp +++ b/examples/cddp_hcw.cpp @@ -147,14 +147,12 @@ int main() { cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP); // Extract solution and print result - const auto& cost_sequence = solution.history.objective; - double J_final = cost_sequence.back(); const auto& X_sol = solution.state_trajectory; const auto& U_sol = solution.control_trajectory; const auto& t_sol = solution.time_points; - + std::cout << "\n[Result] CDDP solved." << std::endl; - std::cout << "[Result] Final cost: " << J_final << std::endl; + std::cout << "[Result] Final cost: " << solution.final_objective << std::endl; std::cout << "[Result] Final state: " << X_sol.back().transpose() << std::endl; diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp index f2d3ec9d..7844579a 100644 --- a/examples/cddp_lti_system.cpp +++ b/examples/cddp_lti_system.cpp @@ -151,10 +151,8 @@ int main() { const auto& X_sol = solution.state_trajectory; const auto& U_sol = solution.control_trajectory; const auto& t_sol = solution.time_points; - const auto& cost_sequence = solution.history.objective; auto iterations = solution.iterations_completed; bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound"); - auto solve_time = static_cast(solution.solve_time_ms); // Create directory for plots if it doesn't exist const std::string plotDirectory = "../results/tests"; @@ -166,9 +164,9 @@ int main() { // Print optimization statistics std::cout << "\nOptimization Results:" << std::endl; std::cout << "Iterations: " << iterations << std::endl; - std::cout << "Final cost: " << cost_sequence.back() << std::endl; + std::cout << "Final cost: " << solution.final_objective << std::endl; std::cout << "Converged: " << (converged ? "Yes" : "No") << std::endl; - std::cout << "Solve time: " << solve_time << " microseconds" << std::endl; + std::cout << "Solve time: " << solution.solve_time_ms << " ms" << std::endl; return 0; } From 608c4268e0d68db1da77ae3709b3cd2a7a70adc2 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 15:34:24 -0400 Subject: [PATCH 6/7] Restore LogDDP regularization-limit-converged semantics The template method hardcoded backward pass regularization exhaustion to NotConverged. LogDDP originally treated this as Converged since the barrier-penalized trajectory is typically still usable. Add virtual handleBackwardPassRegularizationLimit hook with LogDDP override. --- include/cddp-cpp/cddp_core/cddp_solver_base.hpp | 11 +++++++++++ include/cddp-cpp/cddp_core/logddp_solver.hpp | 2 ++ src/cddp_core/cddp_solver_base.cpp | 9 ++++++++- src/cddp_core/logddp_solver.cpp | 8 ++++++++ 4 files changed, 29 insertions(+), 1 deletion(-) diff --git a/include/cddp-cpp/cddp_core/cddp_solver_base.hpp b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp index a4173bfa..85ccfe1e 100644 --- a/include/cddp-cpp/cddp_core/cddp_solver_base.hpp +++ b/include/cddp-cpp/cddp_core/cddp_solver_base.hpp @@ -108,6 +108,17 @@ class CDDPSolverBase : public ISolverAlgorithm { */ virtual void postIterationUpdate(CDDP &context, bool forward_pass_success) {} + /** + * @brief Handle backward pass regularization exhaustion. + * Default: sets termination_reason to "NotConverged" and returns false. + * Override to treat the current solution as usable (e.g., LogDDP returns + * true with "Converged" status since barrier methods may still have a + * usable trajectory). + * @return True if the solution should be considered converged. + */ + virtual bool handleBackwardPassRegularizationLimit( + CDDP &context, std::string &termination_reason); + /** * @brief Handle forward pass failure. Default: increase regularization. * Override for filter restoration (MSIPDDP). diff --git a/include/cddp-cpp/cddp_core/logddp_solver.hpp b/include/cddp-cpp/cddp_core/logddp_solver.hpp index 9e8efff7..9c209f3f 100644 --- a/include/cddp-cpp/cddp_core/logddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/logddp_solver.hpp @@ -41,6 +41,8 @@ class LogDDPSolver : public CDDPSolverBase { protected: // === Virtual hook overrides === void preIterationSetup(CDDP &context) override; + bool handleBackwardPassRegularizationLimit( + CDDP &context, std::string &termination_reason) override; bool backwardPass(CDDP &context) override; ForwardPassResult forwardPass(CDDP &context, double alpha) override; void applyForwardPassResult(CDDP &context, diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index c423245b..f3438ed0 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -95,7 +95,8 @@ CDDPSolution CDDPSolverBase::solve(CDDP &context) { if (!backward_ok) { context.increaseRegularization(); if (context.isRegularizationLimitReached()) { - termination_reason = "RegularizationLimitReached_NotConverged"; + converged = handleBackwardPassRegularizationLimit( + context, termination_reason); if (options.verbose) { std::cerr << getSolverName() << ": Backward pass regularization limit reached" @@ -195,6 +196,12 @@ void CDDPSolverBase::applyForwardPassResult(CDDP &context, context.alpha_du_ = result.alpha_du; } +bool CDDPSolverBase::handleBackwardPassRegularizationLimit( + CDDP &context, std::string &termination_reason) { + termination_reason = "RegularizationLimitReached_NotConverged"; + return false; // not converged +} + bool CDDPSolverBase::handleForwardPassFailure(CDDP &context, std::string &termination_reason) { context.increaseRegularization(); diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index b3d79f56..04fa0b2f 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -211,6 +211,14 @@ void LogDDPSolver::preIterationSetup(CDDP &context) { resetFilter(context); } +bool LogDDPSolver::handleBackwardPassRegularizationLimit( + CDDP & /*context*/, std::string &termination_reason) { + // LogDDP treats regularization exhaustion as converged — the current + // barrier-penalized trajectory is typically still usable. + termination_reason = "RegularizationLimitReached_Converged"; + return true; // converged +} + void LogDDPSolver::applyForwardPassResult(CDDP &context, const ForwardPassResult &result) { // Call base to update X_, U_, cost_, merit_function_, alpha_pr_, alpha_du_ From b4fd1d66431d213eff147d840111584a738ae4f5 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 16:07:23 -0400 Subject: [PATCH 7/7] Restore fail-fast Hessians for nonlinear constraints, clean up PR MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Re-add throwing Hessian overrides for PoleConstraint and SecondOrderConeConstraint (nonlinear — zero Hessians are wrong) - Keep zero-return default in base Constraint (correct for linear) - Remove stale #include from cddp_core.hpp - Remove .claude/setting.json from tracked files --- .claude/setting.json | 50 ----------------------- include/cddp-cpp/cddp_core/cddp_core.hpp | 1 - include/cddp-cpp/cddp_core/constraint.hpp | 34 +++++++++++++++ 3 files changed, 34 insertions(+), 51 deletions(-) delete mode 100644 .claude/setting.json diff --git a/.claude/setting.json b/.claude/setting.json deleted file mode 100644 index 7ef2ce73..00000000 --- a/.claude/setting.json +++ /dev/null @@ -1,50 +0,0 @@ -{ - "tools": { - "bash": { - "disabled_commands": [ - "sudo", - "su", - "passwd", - "usermod", - "groupmod", - "adduser", - "useradd", - "deluser", - "userdel", - "chown", - "chmod", - "mount", - "umount", - "systemctl", - "service", - "apt", - "apt-get", - "dpkg", - "snap", - "pip install --system", - "npm install -g" - ] - } - }, - "security": { - "restrict_privileged_operations": true, - "sandbox_mode": true, - "allowed_patterns": [ - "sudo chown .*github/cddp-cpp.*", - "sudo mkdir -p .*github/cddp-cpp.*", - "sudo chmod .*github/cddp-cpp.*", - "pip install --user .*", - "pip install --target .*", - "pip install --prefix .*", - "pip3 install --user .*", - "pip3 install --target .*", - "pip3 install --prefix .*", - "apt search .*", - "apt show .*", - "apt list .*", - "apt-get update", - "npm install .*", - "docker exec .*" - ] - } -} diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index adbd884d..4ae02106 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -17,7 +17,6 @@ #define CDDP_CDDP_CORE_HPP #include -#include // For std::any (kept for backward compat in printSolutionSummary) #include #include // For std::setw #include // For std::cout, std::cerr diff --git a/include/cddp-cpp/cddp_core/constraint.hpp b/include/cddp-cpp/cddp_core/constraint.hpp index 14ed3efa..6ce5064e 100644 --- a/include/cddp-cpp/cddp_core/constraint.hpp +++ b/include/cddp-cpp/cddp_core/constraint.hpp @@ -594,6 +594,23 @@ namespace cddp return std::max(0.0, g(0)); } + // Hessians not yet implemented for PoleConstraint (nonlinear — zero is wrong) + std::vector + getStateHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getStateHessian not implemented for PoleConstraint."); + } + std::vector + getControlHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getControlHessian not implemented for PoleConstraint."); + } + std::vector + getCrossHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getCrossHessian not implemented for PoleConstraint."); + } + private: Eigen::Vector3d center_; // Center of the cylinder. Eigen::Vector3d axis_; // Unit vector for the cylinder's axis. @@ -761,6 +778,23 @@ namespace cddp return std::max(0.0, g(0)); } + // Hessians not yet implemented for SecondOrderConeConstraint (nonlinear — zero is wrong) + std::vector + getStateHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getStateHessian not implemented for SecondOrderConeConstraint."); + } + std::vector + getControlHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getControlHessian not implemented for SecondOrderConeConstraint."); + } + std::vector + getCrossHessian(const Eigen::VectorXd &, const Eigen::VectorXd &, + int = 0) const override { + throw std::logic_error("getCrossHessian not implemented for SecondOrderConeConstraint."); + } + private: Eigen::Vector3d p_o_; // Cone origin position Eigen::Vector3d