From c91e5182fa638b0ff95270cedd0ebe3c52b40c3f Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 19:24:31 -0400 Subject: [PATCH 01/14] Add Python bindings and make Matplot integration optional This PR adds a pybind11-based pycddp package with bindings for solver options, dynamics, objectives, constraints, and solver execution, along with Python integration tests and packaging metadata. It also makes Matplot-dependent includes and test links optional, adds CMake package installation metadata, and prepares the core library for shared-library consumers such as the Python module. --- .gitignore | 8 + .python-version | 1 + CMakeLists.txt | 93 +++++-- cmake/cddpConfig.cmake.in | 7 + include/cddp-cpp/cddp.hpp | 2 + pyproject.toml | 23 ++ python/CMakeLists.txt | 26 ++ python/pycddp/__init__.py | 73 +++++ python/pycddp/_version.py | 1 + python/src/bind_constraints.cpp | 68 +++++ python/src/bind_dynamics.cpp | 250 ++++++++++++++++++ python/src/bind_objective.cpp | 68 +++++ python/src/bind_options.cpp | 130 +++++++++ python/src/bind_solver.cpp | 96 +++++++ python/src/main.cpp | 18 ++ python/tests/test_all_dynamics.py | 86 ++++++ python/tests/test_constraints.py | 39 +++ python/tests/test_custom_dynamics.py | 42 +++ python/tests/test_options.py | 48 ++++ python/tests/test_pendulum.py | 56 ++++ tests/CMakeLists.txt | 8 +- tests/cddp_core/test_clddp_solver.cpp | 2 + tests/cddp_core/test_constraint.cpp | 6 + tests/cddp_core/test_ipddp_solver.cpp | 2 + tests/cddp_core/test_logddp_solver.cpp | 2 + tests/cddp_core/test_msipddp_core.cpp | 4 + tests/cddp_core/test_msipddp_solver.cpp | 2 + .../dynamics_model/test_attitude_dynamics.cpp | 4 + tests/dynamics_model/test_mrp_attitude.cpp | 4 + .../dynamics_model/test_spacecraft_linear.cpp | 2 + .../test_spacecraft_linear_fuel.cpp | 2 + .../test_spacecraft_nonlinear.cpp | 4 + tests/dynamics_model/test_spacecraft_roe.cpp | 2 + tests/test_matplot.cpp | 2 + 34 files changed, 1157 insertions(+), 24 deletions(-) create mode 100644 .python-version create mode 100644 cmake/cddpConfig.cmake.in create mode 100644 pyproject.toml create mode 100644 python/CMakeLists.txt create mode 100644 python/pycddp/__init__.py create mode 100644 python/pycddp/_version.py create mode 100644 python/src/bind_constraints.cpp create mode 100644 python/src/bind_dynamics.cpp create mode 100644 python/src/bind_objective.cpp create mode 100644 python/src/bind_options.cpp create mode 100644 python/src/bind_solver.cpp create mode 100644 python/src/main.cpp create mode 100644 python/tests/test_all_dynamics.py create mode 100644 python/tests/test_constraints.py create mode 100644 python/tests/test_custom_dynamics.py create mode 100644 python/tests/test_options.py create mode 100644 python/tests/test_pendulum.py diff --git a/.gitignore b/.gitignore index bffdb661..a5716e3c 100644 --- a/.gitignore +++ b/.gitignore @@ -21,3 +21,11 @@ pdf_images/ # Solver output solver_snopt.out + +# Python / uv +.venv/ +uv.lock +*.egg-info/ +__pycache__/ +*.so +*.pyd diff --git a/.python-version b/.python-version new file mode 100644 index 00000000..24ee5b1b --- /dev/null +++ b/.python-version @@ -0,0 +1 @@ +3.13 diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b258bb3..cab74dee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,8 @@ endif() # Options option(CDDP_CPP_BUILD_TESTS "Whether to build tests." ON) +option(CDDP_CPP_BUILD_EXAMPLES "Whether to build examples." ON) +option(CDDP_CPP_ENABLE_PLOTTING "Enable matplotplusplus for visualization." ON) # CasADi Configuration option(CDDP_CPP_CASADI "Whether to use CasADi" OFF) @@ -92,14 +94,16 @@ endif() # Enable FetchContent for downloading dependencies include(FetchContent) -# Matplotplusplus -FetchContent_Declare(matplotplusplus - GIT_REPOSITORY https://github.com/alandefreitas/matplotplusplus - GIT_TAG origin/master) # or whatever tag you want -FetchContent_GetProperties(matplotplusplus) -if(NOT matplotplusplus_POPULATED) - FetchContent_Populate(matplotplusplus) - add_subdirectory(${matplotplusplus_SOURCE_DIR} ${matplotplusplus_BINARY_DIR} EXCLUDE_FROM_ALL) +# Matplotplusplus (optional) +if(CDDP_CPP_ENABLE_PLOTTING) + FetchContent_Declare(matplotplusplus + GIT_REPOSITORY https://github.com/alandefreitas/matplotplusplus + GIT_TAG origin/master) + FetchContent_GetProperties(matplotplusplus) + if(NOT matplotplusplus_POPULATED) + FetchContent_Populate(matplotplusplus) + add_subdirectory(${matplotplusplus_SOURCE_DIR} ${matplotplusplus_BINARY_DIR} EXCLUDE_FROM_ALL) + endif() endif() # autodiff @@ -172,16 +176,23 @@ set(dynamics_model_srcs src/dynamics_model/euler_attitude.cpp ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} ${cddp_core_srcs} ${dynamics_model_srcs} ) -target_link_libraries(${PROJECT_NAME} +# Enable position-independent code (required for Python shared library linking) +set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_link_libraries(${PROJECT_NAME} $,Eigen3::Eigen,> - matplot autodiff -) +) + +if(CDDP_CPP_ENABLE_PLOTTING) + target_link_libraries(${PROJECT_NAME} matplot) + target_compile_definitions(${PROJECT_NAME} PUBLIC CDDP_HAS_MATPLOT) +endif() if(NOT Eigen3_FOUND) target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS}) @@ -258,11 +269,53 @@ if (CDDP_CPP_BUILD_TESTS) endif() # Build examples -add_subdirectory(examples) - -# Cmake compile commmand: -# $ mkdir build -# $ cd build -# $ cmake -DCDDP_CPP_BUILD_TESTS=ON -DCDDP_CPP_CASADI=ON .. -# $ make -j4 -# $ make test +if (CDDP_CPP_BUILD_EXAMPLES) + add_subdirectory(examples) +endif() + +# Python bindings (optional) +option(CDDP_CPP_BUILD_PYTHON "Build Python bindings" OFF) +if(CDDP_CPP_BUILD_PYTHON) + add_subdirectory(python) +endif() + +# Install targets +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) + +install(TARGETS ${PROJECT_NAME} + EXPORT cddpTargets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(DIRECTORY include/cddp-cpp/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/cddp-cpp + FILES_MATCHING PATTERN "*.hpp" +) + +install(EXPORT cddpTargets + FILE cddpTargets.cmake + NAMESPACE cddp:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/cddp +) + +configure_package_config_file( + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/cddpConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/cddpConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/cddp +) + +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/cddpConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/cddpConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/cddpConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/cddp +) diff --git a/cmake/cddpConfig.cmake.in b/cmake/cddpConfig.cmake.in new file mode 100644 index 00000000..39907a3b --- /dev/null +++ b/cmake/cddpConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Eigen3) + +include("${CMAKE_CURRENT_LIST_DIR}/cddpTargets.cmake") +check_required_components(cddp) diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index 1bfe61ad..a7367265 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -57,6 +57,8 @@ #include "dynamics_model/quaternion_attitude.hpp" #include "dynamics_model/mrp_attitude.hpp" +#ifdef CDDP_HAS_MATPLOT #include "matplot/matplot.h" +#endif #endif // CDDP_HPP diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..9820b6de --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,23 @@ +[build-system] +requires = ["scikit-build-core>=0.8", "pybind11>=2.12"] +build-backend = "scikit_build_core.build" + +[project] +name = "pycddp" +version = "0.1.0" +description = "Python bindings for CDDP trajectory optimization" +readme = "README.md" +license = {text = "Apache-2.0"} +requires-python = ">=3.10" +dependencies = ["numpy>=1.22"] + +[project.optional-dependencies] +test = ["pytest>=7.0"] +viz = ["matplotlib>=3.5"] + +[dependency-groups] +dev = ["pytest>=7.0", "matplotlib>=3.5"] + +[tool.scikit-build] +cmake.args = ["-DCDDP_CPP_BUILD_PYTHON=ON", "-DCDDP_CPP_BUILD_TESTS=OFF", "-DCDDP_CPP_BUILD_EXAMPLES=OFF", "-DCDDP_CPP_ENABLE_PLOTTING=OFF"] +wheel.packages = ["python/pycddp"] diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 00000000..9364621c --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,26 @@ +find_package(pybind11 CONFIG QUIET) +if(NOT pybind11_FOUND) + message(STATUS "pybind11 not found, fetching...") + FetchContent_Declare( + pybind11 + GIT_REPOSITORY https://github.com/pybind/pybind11.git + GIT_TAG v2.13.6 + ) + FetchContent_MakeAvailable(pybind11) +endif() + +pybind11_add_module(_pycddp_core + src/main.cpp + src/bind_options.cpp + src/bind_dynamics.cpp + src/bind_objective.cpp + src/bind_constraints.cpp + src/bind_solver.cpp +) + +target_link_libraries(_pycddp_core PRIVATE cddp) +target_include_directories(_pycddp_core PRIVATE + ${CMAKE_SOURCE_DIR}/include/cddp-cpp +) + +install(TARGETS _pycddp_core DESTINATION pycddp) diff --git a/python/pycddp/__init__.py b/python/pycddp/__init__.py new file mode 100644 index 00000000..ff3b6cdc --- /dev/null +++ b/python/pycddp/__init__.py @@ -0,0 +1,73 @@ +"""pycddp - Python bindings for CDDP trajectory optimization.""" + +from pycddp._pycddp_core import ( + # Enums + SolverType, + BarrierStrategy, + + # Options + CDDPOptions, + BoxQPOptions, + LineSearchOptions, + RegularizationOptions, + BarrierOptions, + FilterOptions, + InteriorPointOptions, + LogBarrierOptions, + IPDDPOptions, + MSIPDDPOptions, + + # Core solver + CDDP, + CDDPSolution, + SolutionHistory, + + # Dynamics base + DynamicalSystem, + + # Concrete dynamics models + Pendulum, + Unicycle, + Bicycle, + Car, + CartPole, + DubinsCar, + Forklift, + Acrobot, + Quadrotor, + QuadrotorRate, + Manipulator, + HCW, + SpacecraftLinearFuel, + SpacecraftNonlinear, + DreyfusRocket, + SpacecraftLanding2D, + SpacecraftROE, + SpacecraftTwobody, + LTISystem, + Usv3Dof, + EulerAttitude, + QuaternionAttitude, + MrpAttitude, + + # Objectives + Objective, + QuadraticObjective, + NonlinearObjective, + + # Constraints + Constraint, + ControlConstraint, + StateConstraint, + LinearConstraint, + BallConstraint, + PoleConstraint, + SecondOrderConeConstraint, + ThrustMagnitudeConstraint, + MaxThrustMagnitudeConstraint, + +) + +from pycddp._version import __version__ + +__all__ = [name for name in dir() if not name.startswith("_")] diff --git a/python/pycddp/_version.py b/python/pycddp/_version.py new file mode 100644 index 00000000..3dc1f76b --- /dev/null +++ b/python/pycddp/_version.py @@ -0,0 +1 @@ +__version__ = "0.1.0" diff --git a/python/src/bind_constraints.cpp b/python/src/bind_constraints.cpp new file mode 100644 index 00000000..73ee68c1 --- /dev/null +++ b/python/src/bind_constraints.cpp @@ -0,0 +1,68 @@ +#include +#include +#include + +#include "cddp_core/constraint.hpp" + +namespace py = pybind11; + +template +using nodeleter = std::unique_ptr; + +void bind_constraints(py::module_& m) { + // Base Constraint class - py::nodelete so CDDP can take ownership + py::class_>(m, "Constraint") + .def("evaluate", &cddp::Constraint::evaluate, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("get_lower_bound", &cddp::Constraint::getLowerBound) + .def("get_upper_bound", &cddp::Constraint::getUpperBound) + .def("get_state_jacobian", &cddp::Constraint::getStateJacobian, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("get_control_jacobian", &cddp::Constraint::getControlJacobian, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("compute_violation", &cddp::Constraint::computeViolation, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("get_dual_dim", &cddp::Constraint::getDualDim) + .def_property_readonly("name", &cddp::Constraint::getName); + + py::class_>(m, "ControlConstraint") + .def(py::init(), + py::arg("lower_bound"), py::arg("upper_bound"), + py::arg("scale_factor") = 1.0); + + py::class_>(m, "StateConstraint") + .def(py::init(), + py::arg("lower_bound"), py::arg("upper_bound"), + py::arg("scale_factor") = 1.0); + + py::class_>(m, "LinearConstraint") + .def(py::init(), + py::arg("A"), py::arg("b"), py::arg("scale_factor") = 1.0); + + py::class_>(m, "BallConstraint") + .def(py::init(), + py::arg("radius"), py::arg("center"), + py::arg("scale_factor") = 1.0) + .def("get_center", &cddp::BallConstraint::getCenter); + + py::class_>(m, "PoleConstraint") + .def(py::init(), + py::arg("center"), py::arg("direction"), + py::arg("radius"), py::arg("length"), + py::arg("scale_factor") = 1.0); + + py::class_>(m, "SecondOrderConeConstraint") + .def(py::init(), + py::arg("cone_origin"), py::arg("opening_direction"), + py::arg("cone_angle_fov"), py::arg("epsilon") = 1e-6, + py::arg("name") = "SecondOrderConeConstraint"); + + py::class_>(m, "ThrustMagnitudeConstraint") + .def(py::init(), + py::arg("min_thrust"), py::arg("max_thrust"), + py::arg("epsilon") = 1e-6); + + py::class_>(m, "MaxThrustMagnitudeConstraint") + .def(py::init(), + py::arg("max_thrust"), py::arg("epsilon") = 1e-6); +} diff --git a/python/src/bind_dynamics.cpp b/python/src/bind_dynamics.cpp new file mode 100644 index 00000000..c3770cc7 --- /dev/null +++ b/python/src/bind_dynamics.cpp @@ -0,0 +1,250 @@ +#include +#include +#include + +#include "cddp_core/dynamical_system.hpp" +#include "dynamics_model/pendulum.hpp" +#include "dynamics_model/unicycle.hpp" +#include "dynamics_model/bicycle.hpp" +#include "dynamics_model/car.hpp" +#include "dynamics_model/cartpole.hpp" +#include "dynamics_model/dubins_car.hpp" +#include "dynamics_model/forklift.hpp" +#include "dynamics_model/acrobot.hpp" +#include "dynamics_model/quadrotor.hpp" +#include "dynamics_model/quadrotor_rate.hpp" +#include "dynamics_model/manipulator.hpp" +#include "dynamics_model/spacecraft_linear.hpp" +#include "dynamics_model/spacecraft_linear_fuel.hpp" +#include "dynamics_model/spacecraft_nonlinear.hpp" +#include "dynamics_model/dreyfus_rocket.hpp" +#include "dynamics_model/spacecraft_landing2d.hpp" +#include "dynamics_model/spacecraft_roe.hpp" +#include "dynamics_model/spacecraft_twobody.hpp" +#include "dynamics_model/lti_system.hpp" +#include "dynamics_model/usv_3dof.hpp" +#include "dynamics_model/euler_attitude.hpp" +#include "dynamics_model/quaternion_attitude.hpp" +#include "dynamics_model/mrp_attitude.hpp" + +namespace py = pybind11; + +// Use py::nodelete so Python doesn't free the object - CDDP takes ownership via unique_ptr +template +using nodeleter = std::unique_ptr; + +// Trampoline class for Python subclassing +class PyDynamicalSystem : public cddp::DynamicalSystem { +public: + using cddp::DynamicalSystem::DynamicalSystem; + + Eigen::VectorXd getContinuousDynamics( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(Eigen::VectorXd, cddp::DynamicalSystem, + getContinuousDynamics, state, control, time); + } + + Eigen::VectorXd getDiscreteDynamics( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(Eigen::VectorXd, cddp::DynamicalSystem, + getDiscreteDynamics, state, control, time); + } + + Eigen::MatrixXd getStateJacobian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(Eigen::MatrixXd, cddp::DynamicalSystem, + getStateJacobian, state, control, time); + } + + Eigen::MatrixXd getControlJacobian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(Eigen::MatrixXd, cddp::DynamicalSystem, + getControlJacobian, state, control, time); + } + + std::vector getStateHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, + getStateHessian, state, control, time); + } + + std::vector getControlHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, + getControlHessian, state, control, time); + } + + std::vector getCrossHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + double time) const override { + PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, + getCrossHessian, state, control, time); + } +}; + +void bind_dynamics(py::module_& m) { + // Base class with trampoline - py::nodelete so CDDP can take ownership + py::class_>(m, "DynamicalSystem") + .def(py::init(), + py::arg("state_dim"), py::arg("control_dim"), + py::arg("timestep"), py::arg("integration_type") = "euler") + .def("get_continuous_dynamics", &cddp::DynamicalSystem::getContinuousDynamics, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_discrete_dynamics", &cddp::DynamicalSystem::getDiscreteDynamics, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_state_jacobian", &cddp::DynamicalSystem::getStateJacobian, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_control_jacobian", &cddp::DynamicalSystem::getControlJacobian, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_state_hessian", &cddp::DynamicalSystem::getStateHessian, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_control_hessian", &cddp::DynamicalSystem::getControlHessian, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def("get_cross_hessian", &cddp::DynamicalSystem::getCrossHessian, + py::arg("state"), py::arg("control"), py::arg("time") = 0.0) + .def_property_readonly("state_dim", &cddp::DynamicalSystem::getStateDim) + .def_property_readonly("control_dim", &cddp::DynamicalSystem::getControlDim) + .def_property_readonly("timestep", &cddp::DynamicalSystem::getTimestep) + .def_property_readonly("integration_type", &cddp::DynamicalSystem::getIntegrationType); + + // --- Concrete dynamics models --- + // All use nodeleter so CDDP solver takes ownership + + py::class_>(m, "Pendulum") + .def(py::init(), + py::arg("timestep"), py::arg("length") = 1.0, + py::arg("mass") = 1.0, py::arg("damping") = 0.0, + py::arg("integration_type") = "euler"); + + py::class_>(m, "Unicycle") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "euler"); + + py::class_>(m, "Bicycle") + .def(py::init(), + py::arg("timestep"), py::arg("wheelbase"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "Car") + .def(py::init(), + py::arg("timestep") = 0.03, py::arg("wheelbase") = 2.0, + py::arg("integration_type") = "euler"); + + py::class_>(m, "CartPole") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "rk4", + py::arg("cart_mass") = 1.0, py::arg("pole_mass") = 0.2, + py::arg("pole_length") = 0.5, py::arg("gravity") = 9.81, + py::arg("damping") = 0.0); + + py::class_>(m, "DubinsCar") + .def(py::init(), + py::arg("speed"), py::arg("timestep"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "Forklift") + .def(py::init(), + py::arg("timestep") = 0.01, py::arg("wheelbase") = 2.0, + py::arg("integration_type") = "euler", + py::arg("rear_steer") = true, + py::arg("max_steering_angle") = 0.785398); + + py::class_>(m, "Acrobot") + .def(py::init(), + py::arg("timestep"), py::arg("l1") = 1.0, py::arg("l2") = 1.0, + py::arg("m1") = 1.0, py::arg("m2") = 1.0, + py::arg("J1") = 1.0, py::arg("J2") = 1.0, + py::arg("integration_type") = "euler"); + + py::class_>(m, "Quadrotor") + .def(py::init(), + py::arg("timestep"), py::arg("mass"), + py::arg("inertia_matrix"), py::arg("arm_length"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "QuadrotorRate") + .def(py::init(), + py::arg("timestep"), py::arg("mass"), + py::arg("max_thrust"), py::arg("max_rate"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "Manipulator") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "rk4"); + + py::class_>(m, "HCW") + .def(py::init(), + py::arg("timestep"), py::arg("mean_motion"), + py::arg("mass"), py::arg("integration_type") = "euler"); + + py::class_>(m, "SpacecraftLinearFuel") + .def(py::init(), + py::arg("timestep"), py::arg("mean_motion"), + py::arg("isp"), py::arg("g0") = 9.80665, + py::arg("integration_type") = "euler"); + + py::class_>(m, "SpacecraftNonlinear") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "rk4", + py::arg("mass") = 1.0, py::arg("r_scale") = 1.0, + py::arg("v_scale") = 1.0, py::arg("mu") = 1.0); + + py::class_>(m, "DreyfusRocket") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "rk4", + py::arg("thrust_acceleration") = 64.0, + py::arg("gravity_acceleration") = 32.0); + + py::class_>(m, "SpacecraftLanding2D") + .def(py::init(), + py::arg("timestep") = 0.1, py::arg("integration_type") = "rk4", + py::arg("mass") = 100000.0, py::arg("length") = 50.0, + py::arg("width") = 10.0, py::arg("min_thrust") = 880000.0, + py::arg("max_thrust") = 2210000.0, py::arg("max_gimble") = 0.349066); + + py::class_>(m, "SpacecraftROE") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type"), + py::arg("a"), py::arg("u0") = 0.0, py::arg("mass_kg") = 1.0); + + py::class_>(m, "SpacecraftTwobody") + .def(py::init(), + py::arg("timestep"), py::arg("mu"), py::arg("mass")); + + py::class_>(m, "LTISystem") + .def(py::init(), + py::arg("A"), py::arg("B"), py::arg("timestep"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "Usv3Dof") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type") = "euler"); + + py::class_>(m, "EulerAttitude") + .def(py::init(), + py::arg("timestep"), py::arg("inertia_matrix"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "QuaternionAttitude") + .def(py::init(), + py::arg("timestep"), py::arg("inertia_matrix"), + py::arg("integration_type") = "euler"); + + py::class_>(m, "MrpAttitude") + .def(py::init(), + py::arg("timestep"), py::arg("inertia_matrix"), + py::arg("integration_type") = "euler"); +} diff --git a/python/src/bind_objective.cpp b/python/src/bind_objective.cpp new file mode 100644 index 00000000..ab97a1c8 --- /dev/null +++ b/python/src/bind_objective.cpp @@ -0,0 +1,68 @@ +#include +#include +#include + +#include "cddp_core/objective.hpp" + +namespace py = pybind11; + +template +using nodeleter = std::unique_ptr; + +// Trampoline for NonlinearObjective +class PyNonlinearObjective : public cddp::NonlinearObjective { +public: + using cddp::NonlinearObjective::NonlinearObjective; + + double running_cost(const Eigen::VectorXd& state, + const Eigen::VectorXd& control, + int index) const override { + PYBIND11_OVERRIDE(double, cddp::NonlinearObjective, + running_cost, state, control, index); + } + + double terminal_cost(const Eigen::VectorXd& final_state) const override { + PYBIND11_OVERRIDE(double, cddp::NonlinearObjective, + terminal_cost, final_state); + } + + double evaluate(const std::vector& states, + const std::vector& controls) const override { + PYBIND11_OVERRIDE(double, cddp::NonlinearObjective, + evaluate, states, controls); + } +}; + +void bind_objective(py::module_& m) { + // Base class - py::nodelete so CDDP can take ownership + py::class_>(m, "Objective") + .def("evaluate", &cddp::Objective::evaluate, + py::arg("states"), py::arg("controls")) + .def("running_cost", &cddp::Objective::running_cost, + py::arg("state"), py::arg("control"), py::arg("index")) + .def("terminal_cost", &cddp::Objective::terminal_cost, + py::arg("final_state")) + .def("get_reference_state", &cddp::Objective::getReferenceState) + .def("set_reference_state", &cddp::Objective::setReferenceState, + py::arg("reference_state")) + .def("set_reference_states", &cddp::Objective::setReferenceStates, + py::arg("reference_states")); + + py::class_>(m, "QuadraticObjective") + .def(py::init&, double>(), + py::arg("Q"), py::arg("R"), py::arg("Qf"), + py::arg("reference_state") = Eigen::VectorXd::Zero(0), + py::arg("reference_states") = std::vector(), + py::arg("timestep") = 0.1) + .def_property_readonly("Q", &cddp::QuadraticObjective::getQ) + .def_property_readonly("R", &cddp::QuadraticObjective::getR) + .def_property_readonly("Qf", &cddp::QuadraticObjective::getQf) + .def("set_Q", &cddp::QuadraticObjective::setQ, py::arg("Q")) + .def("set_R", &cddp::QuadraticObjective::setR, py::arg("R")) + .def("set_Qf", &cddp::QuadraticObjective::setQf, py::arg("Qf")); + + py::class_>(m, "NonlinearObjective") + .def(py::init(), py::arg("timestep") = 0.1); +} diff --git a/python/src/bind_options.cpp b/python/src/bind_options.cpp new file mode 100644 index 00000000..a0f2e1a3 --- /dev/null +++ b/python/src/bind_options.cpp @@ -0,0 +1,130 @@ +#include +#include + +#include "cddp_core/options.hpp" +#include "cddp_core/cddp_core.hpp" + +namespace py = pybind11; + +void bind_options(py::module_& m) { + // BarrierStrategy enum + py::enum_(m, "BarrierStrategy") + .value("ADAPTIVE", cddp::BarrierStrategy::ADAPTIVE) + .value("MONOTONIC", cddp::BarrierStrategy::MONOTONIC) + .value("IPOPT", cddp::BarrierStrategy::IPOPT); + + // SolverType enum + py::enum_(m, "SolverType") + .value("CLDDP", cddp::SolverType::CLDDP) + .value("LogDDP", cddp::SolverType::LogDDP) + .value("IPDDP", cddp::SolverType::IPDDP) + .value("MSIPDDP", cddp::SolverType::MSIPDDP); + + // BoxQPOptions + py::class_(m, "BoxQPOptions") + .def(py::init<>()) + .def_readwrite("max_iterations", &cddp::BoxQPOptions::max_iterations) + .def_readwrite("min_gradient_norm", &cddp::BoxQPOptions::min_gradient_norm) + .def_readwrite("min_relative_improvement", &cddp::BoxQPOptions::min_relative_improvement) + .def_readwrite("step_decrease_factor", &cddp::BoxQPOptions::step_decrease_factor) + .def_readwrite("min_step_size", &cddp::BoxQPOptions::min_step_size) + .def_readwrite("armijo_constant", &cddp::BoxQPOptions::armijo_constant) + .def_readwrite("verbose", &cddp::BoxQPOptions::verbose); + + // LineSearchOptions + py::class_(m, "LineSearchOptions") + .def(py::init<>()) + .def_readwrite("max_iterations", &cddp::LineSearchOptions::max_iterations) + .def_readwrite("initial_step_size", &cddp::LineSearchOptions::initial_step_size) + .def_readwrite("min_step_size", &cddp::LineSearchOptions::min_step_size) + .def_readwrite("step_reduction_factor", &cddp::LineSearchOptions::step_reduction_factor); + + // RegularizationOptions + py::class_(m, "RegularizationOptions") + .def(py::init<>()) + .def_readwrite("initial_value", &cddp::RegularizationOptions::initial_value) + .def_readwrite("update_factor", &cddp::RegularizationOptions::update_factor) + .def_readwrite("max_value", &cddp::RegularizationOptions::max_value) + .def_readwrite("min_value", &cddp::RegularizationOptions::min_value) + .def_readwrite("step_initial_value", &cddp::RegularizationOptions::step_initial_value); + + // SolverSpecificBarrierOptions + py::class_(m, "BarrierOptions") + .def(py::init<>()) + .def_readwrite("mu_initial", &cddp::SolverSpecificBarrierOptions::mu_initial) + .def_readwrite("mu_min_value", &cddp::SolverSpecificBarrierOptions::mu_min_value) + .def_readwrite("mu_update_factor", &cddp::SolverSpecificBarrierOptions::mu_update_factor) + .def_readwrite("mu_update_power", &cddp::SolverSpecificBarrierOptions::mu_update_power) + .def_readwrite("min_fraction_to_boundary", &cddp::SolverSpecificBarrierOptions::min_fraction_to_boundary) + .def_readwrite("strategy", &cddp::SolverSpecificBarrierOptions::strategy); + + // SolverSpecificFilterOptions + py::class_(m, "FilterOptions") + .def(py::init<>()) + .def_readwrite("merit_acceptance_threshold", &cddp::SolverSpecificFilterOptions::merit_acceptance_threshold) + .def_readwrite("violation_acceptance_threshold", &cddp::SolverSpecificFilterOptions::violation_acceptance_threshold) + .def_readwrite("max_violation_threshold", &cddp::SolverSpecificFilterOptions::max_violation_threshold) + .def_readwrite("min_violation_for_armijo_check", &cddp::SolverSpecificFilterOptions::min_violation_for_armijo_check) + .def_readwrite("armijo_constant", &cddp::SolverSpecificFilterOptions::armijo_constant); + + // InteriorPointOptions + py::class_(m, "InteriorPointOptions") + .def(py::init<>()) + .def_readwrite("dual_var_init_scale", &cddp::InteriorPointOptions::dual_var_init_scale) + .def_readwrite("slack_var_init_scale", &cddp::InteriorPointOptions::slack_var_init_scale) + .def_readwrite("barrier", &cddp::InteriorPointOptions::barrier); + + // LogBarrierOptions (inherits MultiShootingOptions) + py::class_(m, "LogBarrierOptions") + .def(py::init<>()) + .def_readwrite("segment_length", &cddp::LogBarrierOptions::segment_length) + .def_readwrite("rollout_type", &cddp::LogBarrierOptions::rollout_type) + .def_readwrite("use_controlled_rollout", &cddp::LogBarrierOptions::use_controlled_rollout) + .def_readwrite("costate_var_init_scale", &cddp::LogBarrierOptions::costate_var_init_scale) + .def_readwrite("use_relaxed_log_barrier_penalty", &cddp::LogBarrierOptions::use_relaxed_log_barrier_penalty) + .def_readwrite("relaxed_log_barrier_delta", &cddp::LogBarrierOptions::relaxed_log_barrier_delta) + .def_readwrite("barrier", &cddp::LogBarrierOptions::barrier); + + // IPDDPAlgorithmOptions (inherits InteriorPointOptions) + py::class_(m, "IPDDPOptions") + .def(py::init<>()) + .def_readwrite("dual_var_init_scale", &cddp::IPDDPAlgorithmOptions::dual_var_init_scale) + .def_readwrite("slack_var_init_scale", &cddp::IPDDPAlgorithmOptions::slack_var_init_scale) + .def_readwrite("barrier", &cddp::IPDDPAlgorithmOptions::barrier); + + // MSIPDDPAlgorithmOptions (inherits InteriorPointOptions + MultiShootingOptions) + py::class_(m, "MSIPDDPOptions") + .def(py::init<>()) + .def_readwrite("dual_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::dual_var_init_scale) + .def_readwrite("slack_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::slack_var_init_scale) + .def_readwrite("barrier", &cddp::MSIPDDPAlgorithmOptions::barrier) + .def_readwrite("segment_length", &cddp::MSIPDDPAlgorithmOptions::segment_length) + .def_readwrite("rollout_type", &cddp::MSIPDDPAlgorithmOptions::rollout_type) + .def_readwrite("use_controlled_rollout", &cddp::MSIPDDPAlgorithmOptions::use_controlled_rollout) + .def_readwrite("costate_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::costate_var_init_scale); + + // CDDPOptions + py::class_(m, "CDDPOptions") + .def(py::init<>()) + .def_readwrite("tolerance", &cddp::CDDPOptions::tolerance) + .def_readwrite("acceptable_tolerance", &cddp::CDDPOptions::acceptable_tolerance) + .def_readwrite("max_iterations", &cddp::CDDPOptions::max_iterations) + .def_readwrite("max_cpu_time", &cddp::CDDPOptions::max_cpu_time) + .def_readwrite("verbose", &cddp::CDDPOptions::verbose) + .def_readwrite("debug", &cddp::CDDPOptions::debug) + .def_readwrite("print_solver_header", &cddp::CDDPOptions::print_solver_header) + .def_readwrite("print_solver_options", &cddp::CDDPOptions::print_solver_options) + .def_readwrite("use_ilqr", &cddp::CDDPOptions::use_ilqr) + .def_readwrite("enable_parallel", &cddp::CDDPOptions::enable_parallel) + .def_readwrite("num_threads", &cddp::CDDPOptions::num_threads) + .def_readwrite("return_iteration_info", &cddp::CDDPOptions::return_iteration_info) + .def_readwrite("warm_start", &cddp::CDDPOptions::warm_start) + .def_readwrite("termination_scaling_max_factor", &cddp::CDDPOptions::termination_scaling_max_factor) + .def_readwrite("line_search", &cddp::CDDPOptions::line_search) + .def_readwrite("regularization", &cddp::CDDPOptions::regularization) + .def_readwrite("box_qp", &cddp::CDDPOptions::box_qp) + .def_readwrite("filter", &cddp::CDDPOptions::filter) + .def_readwrite("log_barrier", &cddp::CDDPOptions::log_barrier) + .def_readwrite("ipddp", &cddp::CDDPOptions::ipddp) + .def_readwrite("msipddp", &cddp::CDDPOptions::msipddp); +} diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp new file mode 100644 index 00000000..737243ba --- /dev/null +++ b/python/src/bind_solver.cpp @@ -0,0 +1,96 @@ +#include +#include +#include + +#include "cddp_core/cddp_core.hpp" + +namespace py = pybind11; + +void bind_solver(py::module_& m) { + // CDDPSolution::History + py::class_(m, "SolutionHistory") + .def_readonly("objective", &cddp::CDDPSolution::History::objective) + .def_readonly("merit_function", &cddp::CDDPSolution::History::merit_function) + .def_readonly("step_length_primal", &cddp::CDDPSolution::History::step_length_primal) + .def_readonly("step_length_dual", &cddp::CDDPSolution::History::step_length_dual) + .def_readonly("dual_infeasibility", &cddp::CDDPSolution::History::dual_infeasibility) + .def_readonly("primal_infeasibility", &cddp::CDDPSolution::History::primal_infeasibility) + .def_readonly("complementary_infeasibility", &cddp::CDDPSolution::History::complementary_infeasibility) + .def_readonly("barrier_mu", &cddp::CDDPSolution::History::barrier_mu) + .def_readonly("regularization", &cddp::CDDPSolution::History::regularization); + + // CDDPSolution + py::class_(m, "CDDPSolution") + .def_readonly("solver_name", &cddp::CDDPSolution::solver_name) + .def_readonly("status_message", &cddp::CDDPSolution::status_message) + .def_readonly("iterations_completed", &cddp::CDDPSolution::iterations_completed) + .def_readonly("solve_time_ms", &cddp::CDDPSolution::solve_time_ms) + .def_readonly("final_objective", &cddp::CDDPSolution::final_objective) + .def_readonly("final_step_length", &cddp::CDDPSolution::final_step_length) + .def_readonly("final_regularization", &cddp::CDDPSolution::final_regularization) + .def_readonly("time_points", &cddp::CDDPSolution::time_points) + .def_readonly("state_trajectory", &cddp::CDDPSolution::state_trajectory) + .def_readonly("control_trajectory", &cddp::CDDPSolution::control_trajectory) + .def_readonly("feedback_gains", &cddp::CDDPSolution::feedback_gains) + .def_readonly("final_primal_infeasibility", &cddp::CDDPSolution::final_primal_infeasibility) + .def_readonly("final_dual_infeasibility", &cddp::CDDPSolution::final_dual_infeasibility) + .def_readonly("final_complementary_infeasibility", &cddp::CDDPSolution::final_complementary_infeasibility) + .def_readonly("final_barrier_mu", &cddp::CDDPSolution::final_barrier_mu) + .def_readonly("history", &cddp::CDDPSolution::history); + + // CDDP solver + // Objects are created with py::nodelete holders, so CDDP can safely take + // ownership via unique_ptr without double-free. Python references become + // dangling after passing to the solver - this is documented behavior. + py::class_(m, "CDDP") + .def(py::init([](const Eigen::VectorXd& initial_state, + const Eigen::VectorXd& reference_state, + int horizon, double timestep, + const cddp::CDDPOptions& options) { + return std::make_unique( + initial_state, reference_state, horizon, timestep, + nullptr, nullptr, options); + }), + py::arg("initial_state"), py::arg("reference_state"), + py::arg("horizon"), py::arg("timestep"), + py::arg("options") = cddp::CDDPOptions()) + + .def("set_initial_state", &cddp::CDDP::setInitialState, py::arg("initial_state")) + .def("set_reference_state", &cddp::CDDP::setReferenceState, py::arg("reference_state")) + .def("set_reference_states", &cddp::CDDP::setReferenceStates, py::arg("reference_states")) + .def("set_horizon", &cddp::CDDP::setHorizon, py::arg("horizon")) + .def("set_timestep", &cddp::CDDP::setTimestep, py::arg("timestep")) + .def("set_options", &cddp::CDDP::setOptions, py::arg("options")) + + // Ownership transfer: raw pointer -> unique_ptr (safe because py::nodelete) + .def("set_dynamical_system", [](cddp::CDDP& self, cddp::DynamicalSystem* sys) { + self.setDynamicalSystem(std::unique_ptr(sys)); + }, py::arg("system")) + .def("set_objective", [](cddp::CDDP& self, cddp::Objective* obj) { + self.setObjective(std::unique_ptr(obj)); + }, py::arg("objective")) + .def("add_constraint", [](cddp::CDDP& self, const std::string& name, cddp::Constraint* c) { + self.addPathConstraint(name, std::unique_ptr(c)); + }, py::arg("name"), py::arg("constraint")) + .def("add_terminal_constraint", [](cddp::CDDP& self, const std::string& name, cddp::Constraint* c) { + self.addTerminalConstraint(name, std::unique_ptr(c)); + }, py::arg("name"), py::arg("constraint")) + .def("remove_constraint", &cddp::CDDP::removePathConstraint, py::arg("name")) + .def("remove_terminal_constraint", &cddp::CDDP::removeTerminalConstraint, py::arg("name")) + + .def("set_initial_trajectory", &cddp::CDDP::setInitialTrajectory, + py::arg("X"), py::arg("U")) + + .def("solve", py::overload_cast(&cddp::CDDP::solve), + py::arg("solver_type") = cddp::SolverType::CLDDP) + .def("solve_by_name", py::overload_cast(&cddp::CDDP::solve), + py::arg("solver_type")) + + .def_property_readonly("initial_state", &cddp::CDDP::getInitialState) + .def_property_readonly("reference_state", &cddp::CDDP::getReferenceState) + .def_property_readonly("horizon", &cddp::CDDP::getHorizon) + .def_property_readonly("timestep", &cddp::CDDP::getTimestep) + .def_property_readonly("state_dim", &cddp::CDDP::getStateDim) + .def_property_readonly("control_dim", &cddp::CDDP::getControlDim) + .def_property_readonly("options", &cddp::CDDP::getOptions); +} diff --git a/python/src/main.cpp b/python/src/main.cpp new file mode 100644 index 00000000..92c32ec7 --- /dev/null +++ b/python/src/main.cpp @@ -0,0 +1,18 @@ +#include + +namespace py = pybind11; + +void bind_options(py::module_& m); +void bind_dynamics(py::module_& m); +void bind_objective(py::module_& m); +void bind_constraints(py::module_& m); +void bind_solver(py::module_& m); + +PYBIND11_MODULE(_pycddp_core, m) { + m.doc() = "CDDP: Constrained Differential Dynamic Programming"; + bind_options(m); + bind_dynamics(m); + bind_objective(m); + bind_constraints(m); + bind_solver(m); +} diff --git a/python/tests/test_all_dynamics.py b/python/tests/test_all_dynamics.py new file mode 100644 index 00000000..b3260a9d --- /dev/null +++ b/python/tests/test_all_dynamics.py @@ -0,0 +1,86 @@ +"""Smoke test: construct all dynamics models and call get_discrete_dynamics.""" +import numpy as np +import pycddp + + +def _test_model(model, x, u): + """Verify a dynamics model can compute discrete dynamics and Jacobians.""" + assert model.state_dim == x.shape[0] + assert model.control_dim == u.shape[0] + assert model.timestep > 0 + + x_next = model.get_discrete_dynamics(x, u) + assert x_next.shape == (model.state_dim,) + + A = model.get_state_jacobian(x, u) + assert A.shape == (model.state_dim, model.state_dim) + + B = model.get_control_jacobian(x, u) + assert B.shape == (model.state_dim, model.control_dim) + + +def test_pendulum(): + m = pycddp.Pendulum(0.01, length=1.0, mass=1.0, damping=0.0) + _test_model(m, np.array([0.1, 0.0]), np.array([0.5])) + + +def test_unicycle(): + m = pycddp.Unicycle(0.1) + _test_model(m, np.array([0.0, 0.0, 0.0]), np.array([1.0, 0.1])) + + +def test_bicycle(): + m = pycddp.Bicycle(0.1, wheelbase=2.0) + _test_model(m, np.zeros(4), np.array([1.0, 0.1])) + + +def test_car(): + m = pycddp.Car(0.03, wheelbase=2.0) + _test_model(m, np.zeros(4), np.array([1.0, 0.1])) + + +def test_cartpole(): + m = pycddp.CartPole(0.01) + _test_model(m, np.array([0.0, 0.0, 0.1, 0.0]), np.array([1.0])) + + +def test_dubins_car(): + m = pycddp.DubinsCar(1.0, 0.1) + _test_model(m, np.zeros(3), np.array([0.5])) + + +def test_acrobot(): + m = pycddp.Acrobot(0.01) + _test_model(m, np.zeros(4), np.array([1.0])) + + +def test_manipulator(): + m = pycddp.Manipulator(0.01) + _test_model(m, np.zeros(6), np.array([0.1, 0.1, 0.1])) + + +def test_hcw(): + m = pycddp.HCW(1.0, mean_motion=0.001, mass=1.0) + _test_model(m, np.zeros(6), np.array([0.01, 0.01, 0.01])) + + +def test_spacecraft_linear_fuel(): + m = pycddp.SpacecraftLinearFuel(1.0, mean_motion=0.001, isp=300.0) + _test_model(m, np.zeros(8), np.array([0.01, 0.01, 0.01])) + + +def test_dreyfus_rocket(): + m = pycddp.DreyfusRocket(0.01) + _test_model(m, np.array([0.0, 100.0]), np.array([0.5])) + + +def test_usv_3dof(): + m = pycddp.Usv3Dof(0.1) + _test_model(m, np.zeros(6), np.array([1.0, 0.0, 0.0])) + + +def test_lti_system(): + A = np.array([[0, 1], [-1, 0]]) + B = np.array([[0], [1]]) + m = pycddp.LTISystem(A, B, 0.01) + _test_model(m, np.array([1.0, 0.0]), np.array([0.5])) diff --git a/python/tests/test_constraints.py b/python/tests/test_constraints.py new file mode 100644 index 00000000..834b9b72 --- /dev/null +++ b/python/tests/test_constraints.py @@ -0,0 +1,39 @@ +"""Test constraint construction and evaluation.""" +import numpy as np +import pycddp + + +def test_control_constraint(): + c = pycddp.ControlConstraint(np.array([-1.0, -2.0]), np.array([1.0, 2.0])) + assert c.get_dual_dim() == 4 # 2 lower + 2 upper bounds + assert c.name == "ControlConstraint" + + +def test_state_constraint(): + c = pycddp.StateConstraint(np.array([-5.0, -5.0]), np.array([5.0, 5.0])) + assert c.get_dual_dim() == 4 + + +def test_ball_constraint(): + center = np.array([1.0, 1.0]) + c = pycddp.BallConstraint(radius=0.5, center=center) + assert c.get_dual_dim() == 1 + np.testing.assert_array_equal(c.get_center(), center) + + # Point far from obstacle should satisfy constraint + state_far = np.array([5.0, 5.0]) + control = np.array([0.0]) + val = c.evaluate(state_far, control) + assert val.shape[0] == 1 + + +def test_linear_constraint(): + A = np.array([[1.0, 1.0], [-1.0, 1.0]]) + b = np.array([1.0, 1.0]) + c = pycddp.LinearConstraint(A, b) + assert c.get_dual_dim() == 2 + + state = np.array([0.0, 0.0]) + control = np.array([0.0]) + val = c.evaluate(state, control) + assert val.shape[0] == 2 diff --git a/python/tests/test_custom_dynamics.py b/python/tests/test_custom_dynamics.py new file mode 100644 index 00000000..d79a9a53 --- /dev/null +++ b/python/tests/test_custom_dynamics.py @@ -0,0 +1,42 @@ +"""Test custom dynamics defined in Python via the trampoline.""" +import numpy as np +import pycddp + + +class DoubleIntegrator(pycddp.DynamicalSystem): + """x = [position, velocity], u = [acceleration].""" + def __init__(self, dt): + super().__init__(2, 1, dt, "euler") + + def get_continuous_dynamics(self, state, control, time=0.0): + return np.array([state[1], control[0]]) + + +def test_custom_dynamics_continuous(): + sys = DoubleIntegrator(0.1) + assert sys.state_dim == 2 + assert sys.control_dim == 1 + + x = np.array([0.0, 1.0]) + u = np.array([0.5]) + xdot = sys.get_continuous_dynamics(x, u) + np.testing.assert_allclose(xdot, [1.0, 0.5]) + + +def test_custom_dynamics_discrete(): + """Test discrete dynamics via Euler integration of custom continuous dynamics. + + Note: Calling get_discrete_dynamics on Python-subclassed systems currently + segfaults due to trampoline/nodelete holder interaction. This will be fixed + when the binding is migrated to pybind11 smart_holder. For now, we verify + the continuous dynamics work and compute the Euler step manually. + """ + dt = 0.1 + sys = DoubleIntegrator(dt) + x = np.array([0.0, 1.0]) + u = np.array([0.5]) + + # Verify continuous dynamics work correctly + xdot = sys.get_continuous_dynamics(x, u) + expected_next = x + dt * xdot + np.testing.assert_allclose(expected_next, [0.1, 1.05], atol=1e-10) diff --git a/python/tests/test_options.py b/python/tests/test_options.py new file mode 100644 index 00000000..4ab115f2 --- /dev/null +++ b/python/tests/test_options.py @@ -0,0 +1,48 @@ +"""Test that CDDPOptions and sub-option structs work correctly.""" +import pycddp + + +def test_cdpp_options_defaults(): + opts = pycddp.CDDPOptions() + assert opts.tolerance == 1e-5 + assert opts.max_iterations == 1 + assert opts.verbose is True + assert opts.use_ilqr is True + + +def test_options_modification(): + opts = pycddp.CDDPOptions() + opts.max_iterations = 200 + opts.tolerance = 1e-8 + opts.verbose = False + assert opts.max_iterations == 200 + assert opts.tolerance == 1e-8 + assert opts.verbose is False + + +def test_line_search_options(): + opts = pycddp.CDDPOptions() + opts.line_search.max_iterations = 20 + opts.line_search.step_reduction_factor = 0.3 + assert opts.line_search.max_iterations == 20 + assert abs(opts.line_search.step_reduction_factor - 0.3) < 1e-10 + + +def test_regularization_options(): + opts = pycddp.CDDPOptions() + opts.regularization.initial_value = 1e-4 + opts.regularization.update_factor = 5.0 + assert abs(opts.regularization.initial_value - 1e-4) < 1e-15 + + +def test_solver_type_enum(): + assert pycddp.SolverType.CLDDP is not None + assert pycddp.SolverType.LogDDP is not None + assert pycddp.SolverType.IPDDP is not None + assert pycddp.SolverType.MSIPDDP is not None + + +def test_barrier_strategy_enum(): + assert pycddp.BarrierStrategy.ADAPTIVE is not None + assert pycddp.BarrierStrategy.MONOTONIC is not None + assert pycddp.BarrierStrategy.IPOPT is not None diff --git a/python/tests/test_pendulum.py b/python/tests/test_pendulum.py new file mode 100644 index 00000000..1d681b8e --- /dev/null +++ b/python/tests/test_pendulum.py @@ -0,0 +1,56 @@ +"""Integration test: solve a pendulum swing-up problem.""" +import numpy as np +import pycddp + + +def test_pendulum_swing_up(): + dt = 0.05 + horizon = 50 + x0 = np.array([np.pi, 0.0]) + xref = np.array([0.0, 0.0]) + + Q = np.zeros((2, 2)) + R = 0.1 * np.eye(1) + Qf = 100.0 * np.eye(2) + + opts = pycddp.CDDPOptions() + opts.max_iterations = 100 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(pycddp.Pendulum(dt, length=0.5, mass=1.0, damping=0.01)) + solver.set_objective(pycddp.QuadraticObjective(Q, R, Qf, xref, [], dt)) + solver.add_constraint("ctrl", pycddp.ControlConstraint(np.array([-50.0]), np.array([50.0]))) + + solution = solver.solve(pycddp.SolverType.CLDDP) + + assert solution.iterations_completed > 0 + assert len(solution.state_trajectory) == horizon + 1 + assert len(solution.control_trajectory) == horizon + assert solution.solve_time_ms >= 0 + + +def test_pendulum_logddp(): + dt = 0.05 + horizon = 50 + x0 = np.array([np.pi, 0.0]) + xref = np.array([0.0, 0.0]) + + Q = np.zeros((2, 2)) + R = 0.1 * np.eye(1) + Qf = 100.0 * np.eye(2) + + opts = pycddp.CDDPOptions() + opts.max_iterations = 100 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(pycddp.Pendulum(dt, length=0.5, mass=1.0)) + solver.set_objective(pycddp.QuadraticObjective(Q, R, Qf, xref, [], dt)) + solver.add_constraint("ctrl", pycddp.ControlConstraint(np.array([-50.0]), np.array([50.0]))) + + solution = solver.solve(pycddp.SolverType.LogDDP) + assert solution.iterations_completed > 0 + assert len(solution.state_trajectory) == horizon + 1 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index da67e7de..3a5d02d9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -87,19 +87,19 @@ target_link_libraries(test_usv_3dof gtest gmock gtest_main cddp) gtest_discover_tests(test_usv_3dof) add_executable(test_mrp_attitude dynamics_model/test_mrp_attitude.cpp) -target_link_libraries(test_mrp_attitude gtest gmock gtest_main cddp matplot) +target_link_libraries(test_mrp_attitude gtest gmock gtest_main cddp) gtest_discover_tests(test_mrp_attitude) # add_executable(test_euler_attitude dynamics_model/test_euler_attitude.cpp) -# target_link_libraries(test_euler_attitude gtest gmock gtest_main cddp matplot) +# target_link_libraries(test_euler_attitude gtest gmock gtest_main cddp) # gtest_discover_tests(test_euler_attitude) # add_executable(test_quaternion_attitude dynamics_model/test_quaternion_attitude.cpp) -# target_link_libraries(test_quaternion_attitude gtest gmock gtest_main cddp matplot) +# target_link_libraries(test_quaternion_attitude gtest gmock gtest_main cddp) # gtest_discover_tests(test_quaternion_attitude) add_executable(test_attitude_dynamics dynamics_model/test_attitude_dynamics.cpp) -target_link_libraries(test_attitude_dynamics gtest gmock gtest_main cddp matplot) +target_link_libraries(test_attitude_dynamics gtest gmock gtest_main cddp) gtest_discover_tests(test_attitude_dynamics) add_executable(test_hessian test_hessian.cpp) diff --git a/tests/cddp_core/test_clddp_solver.cpp b/tests/cddp_core/test_clddp_solver.cpp index 8e6cfb4d..36a872e8 100644 --- a/tests/cddp_core/test_clddp_solver.cpp +++ b/tests/cddp_core/test_clddp_solver.cpp @@ -16,7 +16,9 @@ #include #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include #include #include diff --git a/tests/cddp_core/test_constraint.cpp b/tests/cddp_core/test_constraint.cpp index a12b4ec4..1f418629 100644 --- a/tests/cddp_core/test_constraint.cpp +++ b/tests/cddp_core/test_constraint.cpp @@ -17,7 +17,9 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" #include "cddp-cpp/cddp_core/constraint.hpp" +#ifdef CDDP_HAS_MATPLOT #include +#endif #include TEST(ControlConstraintTest, Evaluate) { @@ -234,6 +236,7 @@ TEST(LinearConstraintTest, Evaluate) { } // New test case for LinearConstraint visualization +#ifdef CDDP_HAS_MATPLOT TEST(LinearConstraintTest, Visualization) { namespace plt = matplot; @@ -314,6 +317,7 @@ TEST(LinearConstraintTest, Visualization) { // plt::save(filename); // std::cout << "Saved linear constraint visualization to " << filename << std::endl; } +#endif // CDDP_HAS_MATPLOT // New test suite for SecondOrderConeConstraint TEST(SecondOrderConeConstraintTest, Evaluate) { @@ -394,6 +398,7 @@ TEST(SecondOrderConeConstraintTest, Gradients) { } // New test case for visualization +#ifdef CDDP_HAS_MATPLOT TEST(SecondOrderConeConstraintTest, Visualization) { namespace plt = matplot; @@ -527,6 +532,7 @@ TEST(SecondOrderConeConstraintTest, Visualization) { // plt::save(filename); // std::cout << "Saved cone visualization to " << filename << std::endl; } +#endif // CDDP_HAS_MATPLOT TEST(BallConstraintTest, Hessians) { // Create a ball constraint with a radius of 2.0 and scale_factor 1.0 diff --git a/tests/cddp_core/test_ipddp_solver.cpp b/tests/cddp_core/test_ipddp_solver.cpp index 7e9b99e6..d7514f99 100644 --- a/tests/cddp_core/test_ipddp_solver.cpp +++ b/tests/cddp_core/test_ipddp_solver.cpp @@ -16,7 +16,9 @@ #include #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include #include #include diff --git a/tests/cddp_core/test_logddp_solver.cpp b/tests/cddp_core/test_logddp_solver.cpp index 49728d5b..b6abb5a8 100644 --- a/tests/cddp_core/test_logddp_solver.cpp +++ b/tests/cddp_core/test_logddp_solver.cpp @@ -16,7 +16,9 @@ #include #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include #include #include diff --git a/tests/cddp_core/test_msipddp_core.cpp b/tests/cddp_core/test_msipddp_core.cpp index bec1a407..fdb3f9b3 100644 --- a/tests/cddp_core/test_msipddp_core.cpp +++ b/tests/cddp_core/test_msipddp_core.cpp @@ -24,9 +24,13 @@ #include "gtest/gtest.h" #include "cddp.hpp" +#ifdef CDDP_HAS_MATPLOT #include "matplot/matplot.h" +#endif +#ifdef CDDP_HAS_MATPLOT using namespace matplot; +#endif // namespace fs = std::filesystem; TEST(MSIPDDPTest, Solve) { diff --git a/tests/cddp_core/test_msipddp_solver.cpp b/tests/cddp_core/test_msipddp_solver.cpp index bb2cf607..d26b6fa7 100644 --- a/tests/cddp_core/test_msipddp_solver.cpp +++ b/tests/cddp_core/test_msipddp_solver.cpp @@ -16,7 +16,9 @@ #include #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include #include #include diff --git a/tests/dynamics_model/test_attitude_dynamics.cpp b/tests/dynamics_model/test_attitude_dynamics.cpp index 078039f5..fb823e93 100644 --- a/tests/dynamics_model/test_attitude_dynamics.cpp +++ b/tests/dynamics_model/test_attitude_dynamics.cpp @@ -5,7 +5,9 @@ #include "cddp_core/helper.hpp" // For attitude conversions #include #include +#ifdef CDDP_HAS_MATPLOT #include // For plotting +#endif namespace cddp { @@ -267,7 +269,9 @@ namespace cddp } // Plotting using matplotplusplus +#ifdef CDDP_HAS_MATPLOT using namespace matplot; +#endif // Extract angle components for plotting std::vector yaw_mrp, pitch_mrp, roll_mrp; diff --git a/tests/dynamics_model/test_mrp_attitude.cpp b/tests/dynamics_model/test_mrp_attitude.cpp index 5137129e..a4a792de 100644 --- a/tests/dynamics_model/test_mrp_attitude.cpp +++ b/tests/dynamics_model/test_mrp_attitude.cpp @@ -24,10 +24,14 @@ #include "cddp-cpp/dynamics_model/mrp_attitude.hpp" #include "cddp-cpp/cddp_core/helper.hpp" +#ifdef CDDP_HAS_MATPLOT #include "matplot/matplot.h" +#endif using namespace cddp; +#ifdef CDDP_HAS_MATPLOT using namespace matplot; +#endif // Helper function for skew-symmetric matrix (double) Eigen::Matrix3d skew_double(const Eigen::Vector3d& v) { diff --git a/tests/dynamics_model/test_spacecraft_linear.cpp b/tests/dynamics_model/test_spacecraft_linear.cpp index 0b203e04..fca35fe5 100644 --- a/tests/dynamics_model/test_spacecraft_linear.cpp +++ b/tests/dynamics_model/test_spacecraft_linear.cpp @@ -25,7 +25,9 @@ #include "gtest/gtest.h" #include "dynamics_model/spacecraft_linear.hpp" +#ifdef CDDP_HAS_MATPLOT #include "matplot/matplot.h" +#endif using namespace cddp; diff --git a/tests/dynamics_model/test_spacecraft_linear_fuel.cpp b/tests/dynamics_model/test_spacecraft_linear_fuel.cpp index 16ae1c9d..1494f719 100644 --- a/tests/dynamics_model/test_spacecraft_linear_fuel.cpp +++ b/tests/dynamics_model/test_spacecraft_linear_fuel.cpp @@ -27,8 +27,10 @@ #include "dynamics_model/spacecraft_linear_fuel.hpp" #include "cddp.hpp" using namespace cddp; +#ifdef CDDP_HAS_MATPLOT using namespace matplot; namespace plt = matplot; +#endif TEST(SpacecraftLinearFuelTest, DiscreteDynamics) { // Create an SpacecraftLinearFuel instance diff --git a/tests/dynamics_model/test_spacecraft_nonlinear.cpp b/tests/dynamics_model/test_spacecraft_nonlinear.cpp index 17507fa2..a458ef5c 100644 --- a/tests/dynamics_model/test_spacecraft_nonlinear.cpp +++ b/tests/dynamics_model/test_spacecraft_nonlinear.cpp @@ -21,7 +21,9 @@ #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -167,7 +169,9 @@ TEST(SpacecraftNonlinearTest, AlfriendExample4_1_RelativeMotion) { FAIL() << "Simulation duration is less than one chief orbital period, cannot check periodicity."; } +#ifdef CDDP_HAS_MATPLOT namespace plt = matplot; +#endif std::vector time_plot_data; std::vector px_plot_data, py_plot_data, pz_plot_data; diff --git a/tests/dynamics_model/test_spacecraft_roe.cpp b/tests/dynamics_model/test_spacecraft_roe.cpp index 08a8e139..8b72f3ff 100644 --- a/tests/dynamics_model/test_spacecraft_roe.cpp +++ b/tests/dynamics_model/test_spacecraft_roe.cpp @@ -12,7 +12,9 @@ #include #include #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/spacecraft_linear.hpp" diff --git a/tests/test_matplot.cpp b/tests/test_matplot.cpp index 889fdc0c..566a6991 100644 --- a/tests/test_matplot.cpp +++ b/tests/test_matplot.cpp @@ -1,5 +1,7 @@ #include +#ifdef CDDP_HAS_MATPLOT #include +#endif #include TEST(MatplotTest, BasicPlot) { From 277d523975dcc22b407ce36d9e096f7c2e6c6cbe Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 22:10:07 -0400 Subject: [PATCH 02/14] Fix Docker configure failure from exported Matplot link This keeps matplot and CDDP_HAS_MATPLOT available to in-tree builds while removing them from the installed cddp target interface, which avoids the cddpTargets export error. It also cleans the Docker build step to use explicit CMake and CTest commands and removes the unused Python executable flag. --- CMakeLists.txt | 13 +++++++------ Dockerfile | 21 +++++++++------------ 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cab74dee..08738b18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,13 +185,14 @@ add_library(${PROJECT_NAME} set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) target_link_libraries(${PROJECT_NAME} - $,Eigen3::Eigen,> - autodiff + PUBLIC + $,Eigen3::Eigen,> + autodiff ) if(CDDP_CPP_ENABLE_PLOTTING) - target_link_libraries(${PROJECT_NAME} matplot) - target_compile_definitions(${PROJECT_NAME} PUBLIC CDDP_HAS_MATPLOT) + target_link_libraries(${PROJECT_NAME} PUBLIC $) + target_compile_definitions(${PROJECT_NAME} PUBLIC $) endif() if(NOT Eigen3_FOUND) @@ -205,7 +206,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC if (CDDP_CPP_CASADI) target_include_directories(${PROJECT_NAME} PUBLIC ${CASADI_INCLUDE_DIR}) - target_link_libraries(${PROJECT_NAME} ${CASADI_LIBRARIES}) + target_link_libraries(${PROJECT_NAME} PRIVATE ${CASADI_LIBRARIES}) endif() # ACADOS @@ -252,7 +253,7 @@ if (CDDP_CPP_ACADOS) include_directories(${BLASFEO_INCLUDE_DIR}) include_directories(${HPIPM_INCLUDE_DIR}) link_directories(${ACADOS_LIB_DIR}) - target_link_libraries(${PROJECT_NAME} ${ACADOS_LIBRARIES}) + target_link_libraries(${PROJECT_NAME} PRIVATE ${ACADOS_LIBRARIES}) # Add preprocessor definition to enable ACADOS in code target_compile_definitions(${PROJECT_NAME} PRIVATE CDDP_CPP_ACADOS_ENABLED=1) diff --git a/Dockerfile b/Dockerfile index b52c2e3c..73a45e69 100644 --- a/Dockerfile +++ b/Dockerfile @@ -31,15 +31,12 @@ WORKDIR /app COPY . /app -# # Configure and build your project -RUN rm -rf build && \ - mkdir build && \ - cd build && \ - cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCDDP_CPP_BUILD_TESTS=ON \ - -DCDDP_CPP_CASADI=OFF \ - -DPython_EXECUTABLE=/usr/bin/python3 \ - .. && \ - make -j$(nproc) && \ - make test +# Configure and build the project +RUN rm -rf build && \ + cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DCDDP_CPP_BUILD_TESTS=ON \ + -DCDDP_CPP_BUILD_EXAMPLES=ON \ + -DCDDP_CPP_CASADI=OFF && \ + cmake --build build -j$(nproc) && \ + ctest --test-dir build --output-on-failure From 0fa9842a13bd7107ec131a00143d67ffd87ab287 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 23:53:30 -0400 Subject: [PATCH 03/14] Fix package exports and Python binding ownership This updates the installed CMake and Python package layouts so downstream consumers can find cddp headers and import pycddp after install. It also replaces the unsafe raw-pointer Python ownership handoff with Python-backed solver adapters, acquires the GIL for Python callbacks, and removes the legacy Matplot-driven build surface from the default configuration. --- CMakeLists.txt | 23 +- Dockerfile | 3 - README.md | 131 +-------- cmake/cddpConfig.cmake.in | 1 + examples/CMakeLists.txt | 109 +------ include/cddp-cpp/cddp.hpp | 4 - pyproject.toml | 2 +- python/CMakeLists.txt | 14 +- python/src/bind_constraints.cpp | 22 +- python/src/bind_dynamics.cpp | 276 +++++++++--------- python/src/bind_objective.cpp | 10 +- python/src/bind_solver.cpp | 418 +++++++++++++++++++++++---- python/tests/test_custom_dynamics.py | 60 +++- tests/CMakeLists.txt | 12 +- tests/package_smoke/CMakeLists.txt | 8 + tests/package_smoke/main.cpp | 3 + tests/package_smoke_test.cmake | 43 +++ tests/test_matplot.cpp | 20 -- 18 files changed, 649 insertions(+), 510 deletions(-) create mode 100644 tests/package_smoke/CMakeLists.txt create mode 100644 tests/package_smoke/main.cpp create mode 100644 tests/package_smoke_test.cmake delete mode 100644 tests/test_matplot.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 08738b18..a48da54f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,8 @@ project( HOMEPAGE_URL "https://github.com/astomodynamics/cddp-cpp" ) +include(GNUInstallDirs) + set(CMAKE_CXX_STANDARD 17) # Enforce C++17 as the minimum standard set(CMAKE_CXX_STANDARD_REQUIRED ON) # Enforce C++17 as the minimum standard set(ABSL_PROPAGATE_CXX_STD ON) # Enforce C++17 for absl @@ -44,7 +46,6 @@ endif() # Options option(CDDP_CPP_BUILD_TESTS "Whether to build tests." ON) option(CDDP_CPP_BUILD_EXAMPLES "Whether to build examples." ON) -option(CDDP_CPP_ENABLE_PLOTTING "Enable matplotplusplus for visualization." ON) # CasADi Configuration option(CDDP_CPP_CASADI "Whether to use CasADi" OFF) @@ -94,18 +95,6 @@ endif() # Enable FetchContent for downloading dependencies include(FetchContent) -# Matplotplusplus (optional) -if(CDDP_CPP_ENABLE_PLOTTING) - FetchContent_Declare(matplotplusplus - GIT_REPOSITORY https://github.com/alandefreitas/matplotplusplus - GIT_TAG origin/master) - FetchContent_GetProperties(matplotplusplus) - if(NOT matplotplusplus_POPULATED) - FetchContent_Populate(matplotplusplus) - add_subdirectory(${matplotplusplus_SOURCE_DIR} ${matplotplusplus_BINARY_DIR} EXCLUDE_FROM_ALL) - endif() -endif() - # autodiff set(AUTODIFF_BUILD_TESTS OFF CACHE BOOL "Don't build autodiff tests") set(AUTODIFF_BUILD_EXAMPLES OFF CACHE BOOL "Don't build autodiff examples") @@ -190,18 +179,13 @@ target_link_libraries(${PROJECT_NAME} autodiff ) -if(CDDP_CPP_ENABLE_PLOTTING) - target_link_libraries(${PROJECT_NAME} PUBLIC $) - target_compile_definitions(${PROJECT_NAME} PUBLIC $) -endif() - if(NOT Eigen3_FOUND) target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS}) endif() target_include_directories(${PROJECT_NAME} PUBLIC $ - $ + $ ) if (CDDP_CPP_CASADI) @@ -281,7 +265,6 @@ if(CDDP_CPP_BUILD_PYTHON) endif() # Install targets -include(GNUInstallDirs) include(CMakePackageConfigHelpers) install(TARGETS ${PROJECT_NAME} diff --git a/Dockerfile b/Dockerfile index 73a45e69..52d7b850 100644 --- a/Dockerfile +++ b/Dockerfile @@ -9,9 +9,6 @@ RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-ins curl \ git \ wget \ - libjpeg-dev \ - libpng-dev \ - gnuplot \ libeigen3-dev && \ rm -rf /var/lib/apt/lists/* diff --git a/README.md b/README.md index bb378d9f..5833af92 100644 --- a/README.md +++ b/README.md @@ -29,119 +29,15 @@ $$ $$ ## Examples -### Unicycle - -Simple car-like robot with velocity and steering control: - -```bash -./examples/cddp_unicycle // after building -``` - -Unicycle Model CDDP - -### Unicycle with Obstacle Avoidance - -Simple car-like robot with velocity and steering control: - -```bash -./examples/cddp_unicycle_safe // after building -``` - -Unicycle Model CDDP with Obstacle Avoidance - -### Unicycle with Obstacle Avoidance (infeasible initial guess) - -```bash -./examples/cddp_unicycle_safe_ipddp // after building -``` - -Unicycle Model CDDP with Obstacle Avoidance - -### Bicycle Model - -Bicycle model with velocity and steering control: +The default C++ build currently includes a barrier-strategy comparison example: ```bash -./examples/cddp_bicycle // after building +./examples/test_barrier_strategies ``` -Bicycle Model CDDP - -### Control-limited Car - -Car model with limited velocity and steering control: - -```bash -./examples/cddp_car // after building -``` - -Car Model CDDP - -```bash -./examples/cddp_car_ipddp // after building -``` - -Car Model CDDP - -### Pendulum - -Simple pendulum with torque control: - -```bash -./examples/cddp_pendulum // after building -``` - - -Pendulum CDDP - -### Cartpole - -Cartpole with cart control: - -```bash -./examples/cddp_cartpole // after building -``` - -Cartpole CDDP -Cartpole CDDP - - -### Quadrotor - -Quadrotor with thrust control: - -```bash -./examples/cddp_quadrotor_point // after building -``` - -Quadrotor CDDP - -```bash -./examples/cddp_quadrotor_circle // after building -``` - -Quadrotor CDDP - - -```bash -./examples/cddp_quadrotor_figure_eight_horizontal // after building -``` -Quadrotor CDDP - -```bash -./examples/cddp_quadrotor_figure_eight_vertical // after building -``` -Quadrotor CDDP - -### Manipulator - -Manipulator with joint torque control: - -```bash -./examples/cddp_manipulator // after building -``` - -Manipulator CDDP +Legacy visualization-heavy C++ examples remain in the repository as reference +material, but they are no longer part of the default build. Python bindings are +the supported plotting and notebook workflow. ## Installation ### Dependencies @@ -157,18 +53,6 @@ sudo apt-get install libeigen3-dev # For Ubuntu brew install eigen # For macOS ``` -* [gnuplot](http://www.gnuplot.info/) (Plotting Library) -```bash -sudo apt-get install gnuplot # For Ubuntu -brew install gnuplot # For macOS -``` -* [imagemagick](https://imagemagick.org/) (Image Processing Library) -```bash -sudo apt-get install imagemagick # For Ubuntu -brew install imagemagick # For macOS -``` - - ### Building ```bash git clone -b v0.3.2 https://github.com/astomodynamics/cddp-cpp @@ -192,7 +76,6 @@ If you want to use this library for ROS2 MPC node, please refer [CDDP MPC Packag This library uses the following open-source libraries as core dependencies: -* [matplotplusplus](https://github.com/alandefreitas/matplotplusplus) (MIT License) * [autodiff](https://github.com/autodiff/autodiff) (MIT License) This library also uses the following open-source libraries for optional features: @@ -221,9 +104,9 @@ If you'd like to contribute: If you have ideas for collaboration, want to discuss potential research applications, or have any questions, please feel free to open an issue on GitHub or reach out to the @astomodynamics. We are particularly interested in exploring its use in novel robotic systems and complex motion planning scenarios. ## TODO -* add **python binding** +* improve python binding ergonomics * improve parallelization -* add simulation and its plots +* add simulation examples and Python visualizations * Quadruped robot * Manipulator * Humanoid diff --git a/cmake/cddpConfig.cmake.in b/cmake/cddpConfig.cmake.in index 39907a3b..4b488299 100644 --- a/cmake/cddpConfig.cmake.in +++ b/cmake/cddpConfig.cmake.in @@ -2,6 +2,7 @@ include(CMakeFindDependencyMacro) find_dependency(Eigen3) +find_dependency(autodiff) include("${CMAKE_CURRENT_LIST_DIR}/cddpTargets.cmake") check_required_components(cddp) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index af030631..931dc5ee 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -13,111 +13,10 @@ # limitations under the License. # CMakeLists.txt for the CDDP examples +# +# The legacy visualization-heavy C++ examples are kept in the repository as +# reference material, but they no longer participate in the default build. +# Python bindings are the supported plotting workflow going forward. add_executable(test_barrier_strategies test_barrier_strategies.cpp) target_link_libraries(test_barrier_strategies cddp) - -add_executable(cddp_bicycle cddp_bicycle.cpp) -target_link_libraries(cddp_bicycle cddp) - -add_executable(cddp_car cddp_car.cpp) -target_link_libraries(cddp_car cddp) - -add_executable(cddp_car_ipddp cddp_car_ipddp.cpp) -target_link_libraries(cddp_car_ipddp cddp) - -add_executable(cddp_forklift_ipddp cddp_forklift_ipddp.cpp) -target_link_libraries(cddp_forklift_ipddp cddp) - -add_executable(cddp_cartpole cddp_cartpole.cpp) -target_link_libraries(cddp_cartpole cddp) - -add_executable(cddp_hcw cddp_hcw.cpp) -target_link_libraries(cddp_hcw cddp) - -add_executable(cddp_lti_system cddp_lti_system.cpp) -target_link_libraries(cddp_lti_system cddp) - -add_executable(cddp_manipulator cddp_manipulator.cpp) -target_link_libraries(cddp_manipulator cddp) - -add_executable(cddp_pendulum cddp_pendulum.cpp) -target_link_libraries(cddp_pendulum cddp) - -add_executable(cddp_acrobot cddp_acrobot.cpp) -target_link_libraries(cddp_acrobot cddp) - -add_executable(cddp_quadrotor_circle cddp_quadrotor_circle.cpp) -target_link_libraries(cddp_quadrotor_circle cddp) - -add_executable(cddp_quadrotor_figure_eight_horizontal cddp_quadrotor_figure_eight_horizontal.cpp) -target_link_libraries(cddp_quadrotor_figure_eight_horizontal cddp) - -add_executable(cddp_quadrotor_figure_eight_vertical cddp_quadrotor_figure_eight_vertical.cpp) -target_link_libraries(cddp_quadrotor_figure_eight_vertical cddp) - -add_executable(cddp_quadrotor_point cddp_quadrotor_point.cpp) -target_link_libraries(cddp_quadrotor_point cddp) - -add_executable(cddp_quadrotor_figure_eight_horizontal_safe cddp_quadrotor_figure_eight_horizontal_safe.cpp) -target_link_libraries(cddp_quadrotor_figure_eight_horizontal_safe cddp) - -add_executable(cddp_unicycle cddp_unicycle.cpp) -target_link_libraries(cddp_unicycle cddp) - -add_executable(cddp_unicycle_safe cddp_unicycle_safe.cpp) -target_link_libraries(cddp_unicycle_safe cddp) - -add_executable(cddp_unicycle_safe_ipddp cddp_unicycle_safe_ipddp.cpp) -target_link_libraries(cddp_unicycle_safe_ipddp cddp) - -add_executable(cddp_unicycle_safe_ipddp_v2 cddp_unicycle_safe_ipddp_v2.cpp) -target_link_libraries(cddp_unicycle_safe_ipddp_v2 cddp) - -add_executable(cddp_unicycle_safe_ipddp_v3 cddp_unicycle_safe_ipddp_v3.cpp) -target_link_libraries(cddp_unicycle_safe_ipddp_v3 cddp) - -add_executable(cddp_spacecraft_linear_docking cddp_spacecraft_linear_docking.cpp) -target_link_libraries(cddp_spacecraft_linear_docking cddp) - -add_executable(cddp_spacecraft_linear_rpo cddp_spacecraft_linear_rpo.cpp) -target_link_libraries(cddp_spacecraft_linear_rpo cddp) - -add_executable(cddp_spacecraft_nonlinear_rpo cddp_spacecraft_nonlinear_rpo.cpp) -target_link_libraries(cddp_spacecraft_nonlinear_rpo cddp) - -add_executable(cddp_spacecraft_rpo cddp_spacecraft_rpo.cpp) -target_link_libraries(cddp_spacecraft_rpo cddp) - -add_executable(cddp_spacecraft_rpo_mc cddp_spacecraft_rpo_mc.cpp) -target_link_libraries(cddp_spacecraft_rpo_mc cddp) - -add_executable(cddp_spacecraft_rpo_fuel cddp_spacecraft_rpo_fuel.cpp) -target_link_libraries(cddp_spacecraft_rpo_fuel cddp) - -add_executable(cddp_spacecraft_roe_rpo cddp_spacecraft_roe_rpo.cpp) -target_link_libraries(cddp_spacecraft_roe_rpo cddp) - -add_executable(mpc_hcw mpc_hcw.cpp) -target_link_libraries(mpc_hcw cddp) - -add_executable(cddp_unicycle_mpc cddp_unicycle_mpc.cpp) -target_link_libraries(cddp_unicycle_mpc cddp) - -# Ipopt examples -if (CDDP_CPP_CASADI) - add_executable(ipopt_car ipopt_car.cpp) - target_link_libraries(ipopt_car cddp) - - add_executable(ipopt_unicycle ipopt_unicycle.cpp) - target_link_libraries(ipopt_unicycle cddp) - - add_executable(ipopt_cartpole ipopt_cartpole.cpp) - target_link_libraries(ipopt_cartpole cddp) - - add_executable(ipopt_quadrotor ipopt_quadrotor.cpp) - target_link_libraries(ipopt_quadrotor cddp) - - add_executable(ipopt_spacecrat_linear_fuel ipopt_spacecrat_linear_fuel.cpp) - target_link_libraries(ipopt_spacecrat_linear_fuel cddp) -endif() diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index a7367265..27e6fccf 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -57,8 +57,4 @@ #include "dynamics_model/quaternion_attitude.hpp" #include "dynamics_model/mrp_attitude.hpp" -#ifdef CDDP_HAS_MATPLOT -#include "matplot/matplot.h" -#endif - #endif // CDDP_HPP diff --git a/pyproject.toml b/pyproject.toml index 9820b6de..e059eaa1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -19,5 +19,5 @@ viz = ["matplotlib>=3.5"] dev = ["pytest>=7.0", "matplotlib>=3.5"] [tool.scikit-build] -cmake.args = ["-DCDDP_CPP_BUILD_PYTHON=ON", "-DCDDP_CPP_BUILD_TESTS=OFF", "-DCDDP_CPP_BUILD_EXAMPLES=OFF", "-DCDDP_CPP_ENABLE_PLOTTING=OFF"] +cmake.args = ["-DCDDP_CPP_BUILD_PYTHON=ON", "-DCDDP_CPP_BUILD_TESTS=OFF", "-DCDDP_CPP_BUILD_EXAMPLES=OFF"] wheel.packages = ["python/pycddp"] diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9364621c..d0242225 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,3 +1,5 @@ +find_package(Python3 COMPONENTS Interpreter Development.Module REQUIRED) + find_package(pybind11 CONFIG QUIET) if(NOT pybind11_FOUND) message(STATUS "pybind11 not found, fetching...") @@ -23,4 +25,14 @@ target_include_directories(_pycddp_core PRIVATE ${CMAKE_SOURCE_DIR}/include/cddp-cpp ) -install(TARGETS _pycddp_core DESTINATION pycddp) +set(PYCDDP_INSTALL_DIR + "${CMAKE_INSTALL_LIBDIR}/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages/pycddp" +) + +install(TARGETS _pycddp_core DESTINATION "${PYCDDP_INSTALL_DIR}") +install( + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/__init__.py + ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/_version.py + DESTINATION "${PYCDDP_INSTALL_DIR}" +) diff --git a/python/src/bind_constraints.cpp b/python/src/bind_constraints.cpp index 73ee68c1..6dbe68e1 100644 --- a/python/src/bind_constraints.cpp +++ b/python/src/bind_constraints.cpp @@ -6,12 +6,8 @@ namespace py = pybind11; -template -using nodeleter = std::unique_ptr; - void bind_constraints(py::module_& m) { - // Base Constraint class - py::nodelete so CDDP can take ownership - py::class_>(m, "Constraint") + py::class_(m, "Constraint") .def("evaluate", &cddp::Constraint::evaluate, py::arg("state"), py::arg("control"), py::arg("index") = 0) .def("get_lower_bound", &cddp::Constraint::getLowerBound) @@ -25,44 +21,44 @@ void bind_constraints(py::module_& m) { .def("get_dual_dim", &cddp::Constraint::getDualDim) .def_property_readonly("name", &cddp::Constraint::getName); - py::class_>(m, "ControlConstraint") + py::class_(m, "ControlConstraint") .def(py::init(), py::arg("lower_bound"), py::arg("upper_bound"), py::arg("scale_factor") = 1.0); - py::class_>(m, "StateConstraint") + py::class_(m, "StateConstraint") .def(py::init(), py::arg("lower_bound"), py::arg("upper_bound"), py::arg("scale_factor") = 1.0); - py::class_>(m, "LinearConstraint") + py::class_(m, "LinearConstraint") .def(py::init(), py::arg("A"), py::arg("b"), py::arg("scale_factor") = 1.0); - py::class_>(m, "BallConstraint") + py::class_(m, "BallConstraint") .def(py::init(), py::arg("radius"), py::arg("center"), py::arg("scale_factor") = 1.0) .def("get_center", &cddp::BallConstraint::getCenter); - py::class_>(m, "PoleConstraint") + py::class_(m, "PoleConstraint") .def(py::init(), py::arg("center"), py::arg("direction"), py::arg("radius"), py::arg("length"), py::arg("scale_factor") = 1.0); - py::class_>(m, "SecondOrderConeConstraint") + py::class_(m, "SecondOrderConeConstraint") .def(py::init(), py::arg("cone_origin"), py::arg("opening_direction"), py::arg("cone_angle_fov"), py::arg("epsilon") = 1e-6, py::arg("name") = "SecondOrderConeConstraint"); - py::class_>(m, "ThrustMagnitudeConstraint") + py::class_(m, "ThrustMagnitudeConstraint") .def(py::init(), py::arg("min_thrust"), py::arg("max_thrust"), py::arg("epsilon") = 1e-6); - py::class_>(m, "MaxThrustMagnitudeConstraint") + py::class_(m, "MaxThrustMagnitudeConstraint") .def(py::init(), py::arg("max_thrust"), py::arg("epsilon") = 1e-6); } diff --git a/python/src/bind_dynamics.cpp b/python/src/bind_dynamics.cpp index c3770cc7..4d2caf74 100644 --- a/python/src/bind_dynamics.cpp +++ b/python/src/bind_dynamics.cpp @@ -1,108 +1,104 @@ -#include #include +#include #include #include "cddp_core/dynamical_system.hpp" -#include "dynamics_model/pendulum.hpp" -#include "dynamics_model/unicycle.hpp" +#include "dynamics_model/acrobot.hpp" #include "dynamics_model/bicycle.hpp" #include "dynamics_model/car.hpp" #include "dynamics_model/cartpole.hpp" +#include "dynamics_model/dreyfus_rocket.hpp" #include "dynamics_model/dubins_car.hpp" +#include "dynamics_model/euler_attitude.hpp" #include "dynamics_model/forklift.hpp" -#include "dynamics_model/acrobot.hpp" +#include "dynamics_model/lti_system.hpp" +#include "dynamics_model/manipulator.hpp" +#include "dynamics_model/mrp_attitude.hpp" +#include "dynamics_model/pendulum.hpp" #include "dynamics_model/quadrotor.hpp" #include "dynamics_model/quadrotor_rate.hpp" -#include "dynamics_model/manipulator.hpp" +#include "dynamics_model/quaternion_attitude.hpp" +#include "dynamics_model/spacecraft_landing2d.hpp" #include "dynamics_model/spacecraft_linear.hpp" #include "dynamics_model/spacecraft_linear_fuel.hpp" #include "dynamics_model/spacecraft_nonlinear.hpp" -#include "dynamics_model/dreyfus_rocket.hpp" -#include "dynamics_model/spacecraft_landing2d.hpp" #include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/spacecraft_twobody.hpp" -#include "dynamics_model/lti_system.hpp" +#include "dynamics_model/unicycle.hpp" #include "dynamics_model/usv_3dof.hpp" -#include "dynamics_model/euler_attitude.hpp" -#include "dynamics_model/quaternion_attitude.hpp" -#include "dynamics_model/mrp_attitude.hpp" namespace py = pybind11; -// Use py::nodelete so Python doesn't free the object - CDDP takes ownership via unique_ptr -template -using nodeleter = std::unique_ptr; - -// Trampoline class for Python subclassing class PyDynamicalSystem : public cddp::DynamicalSystem { public: using cddp::DynamicalSystem::DynamicalSystem; - Eigen::VectorXd getContinuousDynamics( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(Eigen::VectorXd, cddp::DynamicalSystem, - getContinuousDynamics, state, control, time); + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(Eigen::VectorXd, cddp::DynamicalSystem, + "get_continuous_dynamics", + getContinuousDynamics, state, control, time); } - Eigen::VectorXd getDiscreteDynamics( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(Eigen::VectorXd, cddp::DynamicalSystem, - getDiscreteDynamics, state, control, time); + Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(Eigen::VectorXd, cddp::DynamicalSystem, + "get_discrete_dynamics", getDiscreteDynamics, + state, control, time); } - Eigen::MatrixXd getStateJacobian( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(Eigen::MatrixXd, cddp::DynamicalSystem, - getStateJacobian, state, control, time); + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(Eigen::MatrixXd, cddp::DynamicalSystem, + "get_state_jacobian", getStateJacobian, state, + control, time); } - Eigen::MatrixXd getControlJacobian( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(Eigen::MatrixXd, cddp::DynamicalSystem, - getControlJacobian, state, control, time); + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(Eigen::MatrixXd, cddp::DynamicalSystem, + "get_control_jacobian", getControlJacobian, + state, control, time); } - std::vector getStateHessian( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, - getStateHessian, state, control, time); + std::vector getStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(std::vector, + cddp::DynamicalSystem, "get_state_hessian", + getStateHessian, state, control, time); } - std::vector getControlHessian( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, - getControlHessian, state, control, time); + std::vector + getControlHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(std::vector, + cddp::DynamicalSystem, "get_control_hessian", + getControlHessian, state, control, time); } - std::vector getCrossHessian( - const Eigen::VectorXd& state, - const Eigen::VectorXd& control, - double time) const override { - PYBIND11_OVERRIDE(std::vector, cddp::DynamicalSystem, - getCrossHessian, state, control, time); + std::vector getCrossHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + PYBIND11_OVERRIDE_NAME(std::vector, + cddp::DynamicalSystem, "get_cross_hessian", + getCrossHessian, state, control, time); } }; -void bind_dynamics(py::module_& m) { - // Base class with trampoline - py::nodelete so CDDP can take ownership - py::class_>(m, "DynamicalSystem") - .def(py::init(), - py::arg("state_dim"), py::arg("control_dim"), - py::arg("timestep"), py::arg("integration_type") = "euler") - .def("get_continuous_dynamics", &cddp::DynamicalSystem::getContinuousDynamics, - py::arg("state"), py::arg("control"), py::arg("time") = 0.0) +void bind_dynamics(py::module_ &m) { + py::class_(m, "DynamicalSystem") + .def(py::init(), py::arg("state_dim"), + py::arg("control_dim"), py::arg("timestep"), + py::arg("integration_type") = "euler") + .def("get_continuous_dynamics", + &cddp::DynamicalSystem::getContinuousDynamics, py::arg("state"), + py::arg("control"), py::arg("time") = 0.0) .def("get_discrete_dynamics", &cddp::DynamicalSystem::getDiscreteDynamics, py::arg("state"), py::arg("control"), py::arg("time") = 0.0) .def("get_state_jacobian", &cddp::DynamicalSystem::getStateJacobian, @@ -116,135 +112,141 @@ void bind_dynamics(py::module_& m) { .def("get_cross_hessian", &cddp::DynamicalSystem::getCrossHessian, py::arg("state"), py::arg("control"), py::arg("time") = 0.0) .def_property_readonly("state_dim", &cddp::DynamicalSystem::getStateDim) - .def_property_readonly("control_dim", &cddp::DynamicalSystem::getControlDim) - .def_property_readonly("timestep", &cddp::DynamicalSystem::getTimestep) - .def_property_readonly("integration_type", &cddp::DynamicalSystem::getIntegrationType); - - // --- Concrete dynamics models --- - // All use nodeleter so CDDP solver takes ownership - - py::class_>(m, "Pendulum") + .def_property_readonly("control_dim", + &cddp::DynamicalSystem::getControlDim) + .def_property_readonly("timestep", + &cddp::DynamicalSystem::getTimestep) + .def_property_readonly("integration_type", + &cddp::DynamicalSystem::getIntegrationType); + + py::class_(m, "Pendulum") .def(py::init(), py::arg("timestep"), py::arg("length") = 1.0, py::arg("mass") = 1.0, py::arg("damping") = 0.0, py::arg("integration_type") = "euler"); - py::class_>(m, "Unicycle") - .def(py::init(), - py::arg("timestep"), py::arg("integration_type") = "euler"); - - py::class_>(m, "Bicycle") - .def(py::init(), - py::arg("timestep"), py::arg("wheelbase"), + py::class_(m, "Unicycle") + .def(py::init(), py::arg("timestep"), py::arg("integration_type") = "euler"); - py::class_>(m, "Car") + py::class_(m, "Bicycle") + .def(py::init(), py::arg("timestep"), + py::arg("wheelbase"), py::arg("integration_type") = "euler"); + + py::class_(m, "Car") .def(py::init(), py::arg("timestep") = 0.03, py::arg("wheelbase") = 2.0, py::arg("integration_type") = "euler"); - py::class_>(m, "CartPole") - .def(py::init(), + py::class_(m, "CartPole") + .def(py::init(), py::arg("timestep"), py::arg("integration_type") = "rk4", py::arg("cart_mass") = 1.0, py::arg("pole_mass") = 0.2, py::arg("pole_length") = 0.5, py::arg("gravity") = 9.81, py::arg("damping") = 0.0); - py::class_>(m, "DubinsCar") - .def(py::init(), - py::arg("speed"), py::arg("timestep"), - py::arg("integration_type") = "euler"); + py::class_(m, "DubinsCar") + .def(py::init(), py::arg("speed"), + py::arg("timestep"), py::arg("integration_type") = "euler"); - py::class_>(m, "Forklift") + py::class_(m, "Forklift") .def(py::init(), py::arg("timestep") = 0.01, py::arg("wheelbase") = 2.0, - py::arg("integration_type") = "euler", - py::arg("rear_steer") = true, + py::arg("integration_type") = "euler", py::arg("rear_steer") = true, py::arg("max_steering_angle") = 0.785398); - py::class_>(m, "Acrobot") - .def(py::init(), + py::class_(m, "Acrobot") + .def(py::init(), py::arg("timestep"), py::arg("l1") = 1.0, py::arg("l2") = 1.0, - py::arg("m1") = 1.0, py::arg("m2") = 1.0, - py::arg("J1") = 1.0, py::arg("J2") = 1.0, - py::arg("integration_type") = "euler"); + py::arg("m1") = 1.0, py::arg("m2") = 1.0, py::arg("J1") = 1.0, + py::arg("J2") = 1.0, py::arg("integration_type") = "euler"); - py::class_>(m, "Quadrotor") - .def(py::init(), + py::class_(m, "Quadrotor") + .def(py::init(), py::arg("timestep"), py::arg("mass"), py::arg("inertia_matrix"), py::arg("arm_length"), py::arg("integration_type") = "euler"); - py::class_>(m, "QuadrotorRate") + py::class_(m, "QuadrotorRate") .def(py::init(), - py::arg("timestep"), py::arg("mass"), - py::arg("max_thrust"), py::arg("max_rate"), - py::arg("integration_type") = "euler"); + py::arg("timestep"), py::arg("mass"), py::arg("max_thrust"), + py::arg("max_rate"), py::arg("integration_type") = "euler"); - py::class_>(m, "Manipulator") - .def(py::init(), - py::arg("timestep"), py::arg("integration_type") = "rk4"); + py::class_(m, "Manipulator") + .def(py::init(), py::arg("timestep"), + py::arg("integration_type") = "rk4"); - py::class_>(m, "HCW") + py::class_(m, "HCW") .def(py::init(), - py::arg("timestep"), py::arg("mean_motion"), - py::arg("mass"), py::arg("integration_type") = "euler"); + py::arg("timestep"), py::arg("mean_motion"), py::arg("mass"), + py::arg("integration_type") = "euler"); - py::class_>(m, "SpacecraftLinearFuel") + py::class_( + m, "SpacecraftLinearFuel") .def(py::init(), - py::arg("timestep"), py::arg("mean_motion"), - py::arg("isp"), py::arg("g0") = 9.80665, + py::arg("timestep"), py::arg("mean_motion"), py::arg("isp"), + py::arg("g0") = 9.80665, py::arg("integration_type") = "euler"); - py::class_>(m, "SpacecraftNonlinear") + py::class_( + m, "SpacecraftNonlinear") .def(py::init(), py::arg("timestep"), py::arg("integration_type") = "rk4", py::arg("mass") = 1.0, py::arg("r_scale") = 1.0, py::arg("v_scale") = 1.0, py::arg("mu") = 1.0); - py::class_>(m, "DreyfusRocket") + py::class_(m, "DreyfusRocket") .def(py::init(), py::arg("timestep"), py::arg("integration_type") = "rk4", py::arg("thrust_acceleration") = 64.0, py::arg("gravity_acceleration") = 32.0); - py::class_>(m, "SpacecraftLanding2D") - .def(py::init(), + py::class_( + m, "SpacecraftLanding2D") + .def(py::init(), py::arg("timestep") = 0.1, py::arg("integration_type") = "rk4", py::arg("mass") = 100000.0, py::arg("length") = 50.0, py::arg("width") = 10.0, py::arg("min_thrust") = 880000.0, - py::arg("max_thrust") = 2210000.0, py::arg("max_gimble") = 0.349066); - - py::class_>(m, "SpacecraftROE") - .def(py::init(), - py::arg("timestep"), py::arg("integration_type"), - py::arg("a"), py::arg("u0") = 0.0, py::arg("mass_kg") = 1.0); - - py::class_>(m, "SpacecraftTwobody") - .def(py::init(), - py::arg("timestep"), py::arg("mu"), py::arg("mass")); - - py::class_>(m, "LTISystem") - .def(py::init(), + py::arg("max_thrust") = 2210000.0, + py::arg("max_gimble") = 0.349066); + + py::class_(m, "SpacecraftROE") + .def(py::init(), + py::arg("timestep"), py::arg("integration_type"), py::arg("a"), + py::arg("u0") = 0.0, py::arg("mass_kg") = 1.0); + + py::class_( + m, "SpacecraftTwobody") + .def(py::init(), py::arg("timestep"), + py::arg("mu"), py::arg("mass")); + + py::class_(m, "LTISystem") + .def(py::init(), py::arg("A"), py::arg("B"), py::arg("timestep"), py::arg("integration_type") = "euler"); - py::class_>(m, "Usv3Dof") - .def(py::init(), - py::arg("timestep"), py::arg("integration_type") = "euler"); + py::class_(m, "Usv3Dof") + .def(py::init(), py::arg("timestep"), + py::arg("integration_type") = "euler"); - py::class_>(m, "EulerAttitude") - .def(py::init(), + py::class_(m, "EulerAttitude") + .def(py::init(), py::arg("timestep"), py::arg("inertia_matrix"), py::arg("integration_type") = "euler"); - py::class_>(m, "QuaternionAttitude") - .def(py::init(), + py::class_( + m, "QuaternionAttitude") + .def(py::init(), py::arg("timestep"), py::arg("inertia_matrix"), py::arg("integration_type") = "euler"); - py::class_>(m, "MrpAttitude") - .def(py::init(), + py::class_(m, "MrpAttitude") + .def(py::init(), py::arg("timestep"), py::arg("inertia_matrix"), py::arg("integration_type") = "euler"); } diff --git a/python/src/bind_objective.cpp b/python/src/bind_objective.cpp index ab97a1c8..9c78f09f 100644 --- a/python/src/bind_objective.cpp +++ b/python/src/bind_objective.cpp @@ -6,9 +6,6 @@ namespace py = pybind11; -template -using nodeleter = std::unique_ptr; - // Trampoline for NonlinearObjective class PyNonlinearObjective : public cddp::NonlinearObjective { public: @@ -34,8 +31,7 @@ class PyNonlinearObjective : public cddp::NonlinearObjective { }; void bind_objective(py::module_& m) { - // Base class - py::nodelete so CDDP can take ownership - py::class_>(m, "Objective") + py::class_(m, "Objective") .def("evaluate", &cddp::Objective::evaluate, py::arg("states"), py::arg("controls")) .def("running_cost", &cddp::Objective::running_cost, @@ -48,7 +44,7 @@ void bind_objective(py::module_& m) { .def("set_reference_states", &cddp::Objective::setReferenceStates, py::arg("reference_states")); - py::class_>(m, "QuadraticObjective") + py::class_(m, "QuadraticObjective") .def(py::init&, double>(), @@ -63,6 +59,6 @@ void bind_objective(py::module_& m) { .def("set_R", &cddp::QuadraticObjective::setR, py::arg("R")) .def("set_Qf", &cddp::QuadraticObjective::setQf, py::arg("Qf")); - py::class_>(m, "NonlinearObjective") + py::class_(m, "NonlinearObjective") .def(py::init(), py::arg("timestep") = 0.1); } diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 737243ba..ee26f134 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -1,93 +1,391 @@ -#include #include +#include #include #include "cddp_core/cddp_core.hpp" namespace py = pybind11; -void bind_solver(py::module_& m) { - // CDDPSolution::History +namespace { + +class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { +public: + PythonBackedDynamicalSystem(py::object owner, cddp::DynamicalSystem *wrapped) + : cddp::DynamicalSystem(wrapped->getStateDim(), wrapped->getControlDim(), + wrapped->getTimestep(), + wrapped->getIntegrationType()), + owner_(std::move(owner)), wrapped_(wrapped) {} + + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getContinuousDynamics(state, control, time); + } + + cddp::VectorXdual2nd + getContinuousDynamicsAutodiff(const cddp::VectorXdual2nd &state, + const cddp::VectorXdual2nd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getContinuousDynamicsAutodiff(state, control, time); + } + + Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getDiscreteDynamics(state, control, time); + } + + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getStateJacobian(state, control, time); + } + + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getControlJacobian(state, control, time); + } + + std::vector getStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getStateHessian(state, control, time); + } + + std::vector + getControlHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getControlHessian(state, control, time); + } + + std::vector getCrossHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + py::gil_scoped_acquire gil; + return wrapped_->getCrossHessian(state, control, time); + } + +private: + py::object owner_; + cddp::DynamicalSystem *wrapped_; +}; + +class PythonBackedObjective : public cddp::Objective { +public: + PythonBackedObjective(py::object owner, cddp::Objective *wrapped) + : owner_(std::move(owner)), wrapped_(wrapped) {} + + double evaluate(const std::vector &states, + const std::vector &controls) const override { + py::gil_scoped_acquire gil; + return wrapped_->evaluate(states, controls); + } + + double running_cost(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->running_cost(state, control, index); + } + + double terminal_cost(const Eigen::VectorXd &final_state) const override { + py::gil_scoped_acquire gil; + return wrapped_->terminal_cost(final_state); + } + + Eigen::VectorXd + getRunningCostStateGradient(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getRunningCostStateGradient(state, control, index); + } + + Eigen::VectorXd + getRunningCostControlGradient(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getRunningCostControlGradient(state, control, index); + } + + Eigen::VectorXd + getFinalCostGradient(const Eigen::VectorXd &final_state) const override { + py::gil_scoped_acquire gil; + return wrapped_->getFinalCostGradient(final_state); + } + + Eigen::MatrixXd + getRunningCostStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getRunningCostStateHessian(state, control, index); + } + + Eigen::MatrixXd + getRunningCostControlHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getRunningCostControlHessian(state, control, index); + } + + Eigen::MatrixXd + getRunningCostCrossHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getRunningCostCrossHessian(state, control, index); + } + + Eigen::MatrixXd + getFinalCostHessian(const Eigen::VectorXd &final_state) const override { + py::gil_scoped_acquire gil; + return wrapped_->getFinalCostHessian(final_state); + } + + Eigen::VectorXd getReferenceState() const override { + py::gil_scoped_acquire gil; + return wrapped_->getReferenceState(); + } + + std::vector getReferenceStates() const override { + py::gil_scoped_acquire gil; + return wrapped_->getReferenceStates(); + } + + void setReferenceState(const Eigen::VectorXd &reference_state) override { + py::gil_scoped_acquire gil; + wrapped_->setReferenceState(reference_state); + } + + void setReferenceStates( + const std::vector &reference_states) override { + py::gil_scoped_acquire gil; + wrapped_->setReferenceStates(reference_states); + } + +private: + py::object owner_; + cddp::Objective *wrapped_; +}; + +class PythonBackedConstraint : public cddp::Constraint { +public: + PythonBackedConstraint(py::object owner, cddp::Constraint *wrapped) + : cddp::Constraint(wrapped->getName()), owner_(std::move(owner)), + wrapped_(wrapped) {} + + int getDualDim() const override { + py::gil_scoped_acquire gil; + return wrapped_->getDualDim(); + } + + + Eigen::VectorXd evaluate(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->evaluate(state, control, index); + } + + Eigen::VectorXd getLowerBound() const override { + py::gil_scoped_acquire gil; + return wrapped_->getLowerBound(); + } + + Eigen::VectorXd getUpperBound() const override { + py::gil_scoped_acquire gil; + return wrapped_->getUpperBound(); + } + + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getStateJacobian(state, control, index); + } + + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getControlJacobian(state, control, index); + } + + double computeViolation(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->computeViolation(state, control, index); + } + + double computeViolationFromValue(const Eigen::VectorXd &g) const override { + py::gil_scoped_acquire gil; + return wrapped_->computeViolationFromValue(g); + } + + Eigen::VectorXd getCenter() const override { + py::gil_scoped_acquire gil; + return wrapped_->getCenter(); + } + + std::vector getStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getStateHessian(state, control, index); + } + + std::vector + getControlHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getControlHessian(state, control, index); + } + + std::vector getCrossHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + py::gil_scoped_acquire gil; + return wrapped_->getCrossHessian(state, control, index); + } + +private: + py::object owner_; + cddp::Constraint *wrapped_; +}; + +} // namespace + +void bind_solver(py::module_ &m) { py::class_(m, "SolutionHistory") .def_readonly("objective", &cddp::CDDPSolution::History::objective) - .def_readonly("merit_function", &cddp::CDDPSolution::History::merit_function) - .def_readonly("step_length_primal", &cddp::CDDPSolution::History::step_length_primal) - .def_readonly("step_length_dual", &cddp::CDDPSolution::History::step_length_dual) - .def_readonly("dual_infeasibility", &cddp::CDDPSolution::History::dual_infeasibility) - .def_readonly("primal_infeasibility", &cddp::CDDPSolution::History::primal_infeasibility) - .def_readonly("complementary_infeasibility", &cddp::CDDPSolution::History::complementary_infeasibility) + .def_readonly("merit_function", + &cddp::CDDPSolution::History::merit_function) + .def_readonly("step_length_primal", + &cddp::CDDPSolution::History::step_length_primal) + .def_readonly("step_length_dual", + &cddp::CDDPSolution::History::step_length_dual) + .def_readonly("dual_infeasibility", + &cddp::CDDPSolution::History::dual_infeasibility) + .def_readonly("primal_infeasibility", + &cddp::CDDPSolution::History::primal_infeasibility) + .def_readonly("complementary_infeasibility", + &cddp::CDDPSolution::History::complementary_infeasibility) .def_readonly("barrier_mu", &cddp::CDDPSolution::History::barrier_mu) - .def_readonly("regularization", &cddp::CDDPSolution::History::regularization); + .def_readonly("regularization", + &cddp::CDDPSolution::History::regularization); - // CDDPSolution py::class_(m, "CDDPSolution") .def_readonly("solver_name", &cddp::CDDPSolution::solver_name) .def_readonly("status_message", &cddp::CDDPSolution::status_message) - .def_readonly("iterations_completed", &cddp::CDDPSolution::iterations_completed) + .def_readonly("iterations_completed", + &cddp::CDDPSolution::iterations_completed) .def_readonly("solve_time_ms", &cddp::CDDPSolution::solve_time_ms) .def_readonly("final_objective", &cddp::CDDPSolution::final_objective) - .def_readonly("final_step_length", &cddp::CDDPSolution::final_step_length) - .def_readonly("final_regularization", &cddp::CDDPSolution::final_regularization) + .def_readonly("final_step_length", + &cddp::CDDPSolution::final_step_length) + .def_readonly("final_regularization", + &cddp::CDDPSolution::final_regularization) .def_readonly("time_points", &cddp::CDDPSolution::time_points) - .def_readonly("state_trajectory", &cddp::CDDPSolution::state_trajectory) - .def_readonly("control_trajectory", &cddp::CDDPSolution::control_trajectory) + .def_readonly("state_trajectory", + &cddp::CDDPSolution::state_trajectory) + .def_readonly("control_trajectory", + &cddp::CDDPSolution::control_trajectory) .def_readonly("feedback_gains", &cddp::CDDPSolution::feedback_gains) - .def_readonly("final_primal_infeasibility", &cddp::CDDPSolution::final_primal_infeasibility) - .def_readonly("final_dual_infeasibility", &cddp::CDDPSolution::final_dual_infeasibility) - .def_readonly("final_complementary_infeasibility", &cddp::CDDPSolution::final_complementary_infeasibility) - .def_readonly("final_barrier_mu", &cddp::CDDPSolution::final_barrier_mu) + .def_readonly("final_primal_infeasibility", + &cddp::CDDPSolution::final_primal_infeasibility) + .def_readonly("final_dual_infeasibility", + &cddp::CDDPSolution::final_dual_infeasibility) + .def_readonly("final_complementary_infeasibility", + &cddp::CDDPSolution::final_complementary_infeasibility) + .def_readonly("final_barrier_mu", + &cddp::CDDPSolution::final_barrier_mu) .def_readonly("history", &cddp::CDDPSolution::history); - // CDDP solver - // Objects are created with py::nodelete holders, so CDDP can safely take - // ownership via unique_ptr without double-free. Python references become - // dangling after passing to the solver - this is documented behavior. py::class_(m, "CDDP") - .def(py::init([](const Eigen::VectorXd& initial_state, - const Eigen::VectorXd& reference_state, - int horizon, double timestep, - const cddp::CDDPOptions& options) { - return std::make_unique( - initial_state, reference_state, horizon, timestep, - nullptr, nullptr, options); - }), - py::arg("initial_state"), py::arg("reference_state"), - py::arg("horizon"), py::arg("timestep"), - py::arg("options") = cddp::CDDPOptions()) - - .def("set_initial_state", &cddp::CDDP::setInitialState, py::arg("initial_state")) - .def("set_reference_state", &cddp::CDDP::setReferenceState, py::arg("reference_state")) - .def("set_reference_states", &cddp::CDDP::setReferenceStates, py::arg("reference_states")) + .def(py::init([](const Eigen::VectorXd &initial_state, + const Eigen::VectorXd &reference_state, int horizon, + double timestep, const cddp::CDDPOptions &options) { + return std::make_unique(initial_state, + reference_state, horizon, + timestep, nullptr, nullptr, + options); + }), + py::arg("initial_state"), py::arg("reference_state"), + py::arg("horizon"), py::arg("timestep"), + py::arg("options") = cddp::CDDPOptions()) + + .def("set_initial_state", &cddp::CDDP::setInitialState, + py::arg("initial_state")) + .def("set_reference_state", &cddp::CDDP::setReferenceState, + py::arg("reference_state")) + .def("set_reference_states", &cddp::CDDP::setReferenceStates, + py::arg("reference_states")) .def("set_horizon", &cddp::CDDP::setHorizon, py::arg("horizon")) .def("set_timestep", &cddp::CDDP::setTimestep, py::arg("timestep")) .def("set_options", &cddp::CDDP::setOptions, py::arg("options")) - - // Ownership transfer: raw pointer -> unique_ptr (safe because py::nodelete) - .def("set_dynamical_system", [](cddp::CDDP& self, cddp::DynamicalSystem* sys) { - self.setDynamicalSystem(std::unique_ptr(sys)); - }, py::arg("system")) - .def("set_objective", [](cddp::CDDP& self, cddp::Objective* obj) { - self.setObjective(std::unique_ptr(obj)); - }, py::arg("objective")) - .def("add_constraint", [](cddp::CDDP& self, const std::string& name, cddp::Constraint* c) { - self.addPathConstraint(name, std::unique_ptr(c)); - }, py::arg("name"), py::arg("constraint")) - .def("add_terminal_constraint", [](cddp::CDDP& self, const std::string& name, cddp::Constraint* c) { - self.addTerminalConstraint(name, std::unique_ptr(c)); - }, py::arg("name"), py::arg("constraint")) - .def("remove_constraint", &cddp::CDDP::removePathConstraint, py::arg("name")) - .def("remove_terminal_constraint", &cddp::CDDP::removeTerminalConstraint, py::arg("name")) - + .def("set_dynamical_system", + [](cddp::CDDP &self, py::object system) { + auto *wrapped = system.cast(); + self.setDynamicalSystem( + std::make_unique( + std::move(system), wrapped)); + }, + py::arg("system"), + "Keep a Python-owned dynamics model alive inside the solver.") + .def("set_objective", + [](cddp::CDDP &self, py::object objective) { + auto *wrapped = objective.cast(); + self.setObjective(std::make_unique( + std::move(objective), wrapped)); + }, + py::arg("objective"), + "Keep a Python-owned objective alive inside the solver.") + .def("add_constraint", + [](cddp::CDDP &self, const std::string &name, py::object constraint) { + auto *wrapped = constraint.cast(); + self.addPathConstraint(name, + std::make_unique( + std::move(constraint), wrapped)); + }, + py::arg("name"), py::arg("constraint"), + "Keep a Python-owned path constraint alive inside the solver.") + .def("add_terminal_constraint", + [](cddp::CDDP &self, const std::string &name, py::object constraint) { + auto *wrapped = constraint.cast(); + self.addTerminalConstraint( + name, std::make_unique( + std::move(constraint), wrapped)); + }, + py::arg("name"), py::arg("constraint"), + "Keep a Python-owned terminal constraint alive inside the solver.") + .def("remove_constraint", &cddp::CDDP::removePathConstraint, + py::arg("name")) + .def("remove_terminal_constraint", + &cddp::CDDP::removeTerminalConstraint, py::arg("name")) .def("set_initial_trajectory", &cddp::CDDP::setInitialTrajectory, py::arg("X"), py::arg("U")) - .def("solve", py::overload_cast(&cddp::CDDP::solve), py::arg("solver_type") = cddp::SolverType::CLDDP) - .def("solve_by_name", py::overload_cast(&cddp::CDDP::solve), + .def("solve_by_name", + py::overload_cast(&cddp::CDDP::solve), py::arg("solver_type")) - .def_property_readonly("initial_state", &cddp::CDDP::getInitialState) - .def_property_readonly("reference_state", &cddp::CDDP::getReferenceState) + .def_property_readonly("reference_state", + &cddp::CDDP::getReferenceState) .def_property_readonly("horizon", &cddp::CDDP::getHorizon) .def_property_readonly("timestep", &cddp::CDDP::getTimestep) .def_property_readonly("state_dim", &cddp::CDDP::getStateDim) diff --git a/python/tests/test_custom_dynamics.py b/python/tests/test_custom_dynamics.py index d79a9a53..4c58ff72 100644 --- a/python/tests/test_custom_dynamics.py +++ b/python/tests/test_custom_dynamics.py @@ -11,6 +11,21 @@ def __init__(self, dt): def get_continuous_dynamics(self, state, control, time=0.0): return np.array([state[1], control[0]]) + def get_state_jacobian(self, state, control, time=0.0): + return np.array([[0.0, 1.0], [0.0, 0.0]]) + + def get_control_jacobian(self, state, control, time=0.0): + return np.array([[0.0], [1.0]]) + + def get_state_hessian(self, state, control, time=0.0): + return [np.zeros((2, 2)), np.zeros((2, 2))] + + def get_control_hessian(self, state, control, time=0.0): + return [np.zeros((1, 1)), np.zeros((1, 1))] + + def get_cross_hessian(self, state, control, time=0.0): + return [np.zeros((1, 2)), np.zeros((1, 2))] + def test_custom_dynamics_continuous(): sys = DoubleIntegrator(0.1) @@ -24,19 +39,42 @@ def test_custom_dynamics_continuous(): def test_custom_dynamics_discrete(): - """Test discrete dynamics via Euler integration of custom continuous dynamics. - - Note: Calling get_discrete_dynamics on Python-subclassed systems currently - segfaults due to trampoline/nodelete holder interaction. This will be fixed - when the binding is migrated to pybind11 smart_holder. For now, we verify - the continuous dynamics work and compute the Euler step manually. - """ + """Test discrete dynamics via Euler integration of custom continuous dynamics.""" dt = 0.1 sys = DoubleIntegrator(dt) x = np.array([0.0, 1.0]) u = np.array([0.5]) - # Verify continuous dynamics work correctly - xdot = sys.get_continuous_dynamics(x, u) - expected_next = x + dt * xdot - np.testing.assert_allclose(expected_next, [0.1, 1.05], atol=1e-10) + expected_next = np.array([0.1, 1.05]) + x_next = sys.get_discrete_dynamics(x, u) + np.testing.assert_allclose(x_next, expected_next, atol=1e-10) + + +def test_custom_dynamics_with_solver(): + dt = 0.1 + horizon = 20 + x0 = np.array([1.0, 0.0]) + xref = np.array([0.0, 0.0]) + + Q = np.zeros((2, 2)) + R = 0.1 * np.eye(1) + Qf = 10.0 * np.eye(2) + + opts = pycddp.CDDPOptions() + opts.max_iterations = 30 + opts.verbose = False + opts.print_solver_header = False + opts.enable_parallel = True + opts.num_threads = 2 + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system(DoubleIntegrator(dt)) + solver.set_objective(pycddp.QuadraticObjective(Q, R, Qf, xref, [], dt)) + solver.add_constraint("ctrl", pycddp.ControlConstraint(np.array([-5.0]), np.array([5.0]))) + + solution = solver.solve(pycddp.SolverType.CLDDP) + + assert solution.iterations_completed >= 0 + assert len(solution.state_trajectory) == horizon + 1 + assert len(solution.control_trajectory) == horizon + assert solution.solve_time_ms >= 0 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 3a5d02d9..68fb1003 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -175,10 +175,6 @@ gtest_discover_tests(test_msipddp_solver) # target_link_libraries(test_spacecraft_roe gtest gmock gtest_main cddp) # gtest_discover_tests(test_spacecraft_roe) -# add_executable(test_matplot test_matplot.cpp) -# target_link_libraries(test_matplot gtest gmock gtest_main cddp) -# gtest_discover_tests(test_matplot) - if (CDDP_CPP_CASADI) add_executable(test_casadi test_casadi_solver.cpp) target_link_libraries(test_casadi gtest gmock gtest_main cddp) @@ -194,3 +190,11 @@ gtest_discover_tests(test_eigen) add_executable(test_autodiff test_autodiff.cpp) target_link_libraries(test_autodiff gtest gmock gtest_main cddp) gtest_discover_tests(test_autodiff) + +add_test( + NAME PackageInstallSmoke + COMMAND ${CMAKE_COMMAND} + -DCDDP_BINARY_DIR=${CMAKE_BINARY_DIR} + -DCDDP_SOURCE_DIR=${CMAKE_SOURCE_DIR} + -P ${CMAKE_SOURCE_DIR}/tests/package_smoke_test.cmake +) diff --git a/tests/package_smoke/CMakeLists.txt b/tests/package_smoke/CMakeLists.txt new file mode 100644 index 00000000..4c4fee71 --- /dev/null +++ b/tests/package_smoke/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.14) + +project(cddp_package_smoke LANGUAGES CXX) + +find_package(cddp REQUIRED) + +add_executable(cddp_package_smoke main.cpp) +target_link_libraries(cddp_package_smoke PRIVATE cddp::cddp) diff --git a/tests/package_smoke/main.cpp b/tests/package_smoke/main.cpp new file mode 100644 index 00000000..f443ec45 --- /dev/null +++ b/tests/package_smoke/main.cpp @@ -0,0 +1,3 @@ +#include "cddp.hpp" + +int main() { return 0; } diff --git a/tests/package_smoke_test.cmake b/tests/package_smoke_test.cmake new file mode 100644 index 00000000..8320b715 --- /dev/null +++ b/tests/package_smoke_test.cmake @@ -0,0 +1,43 @@ +if(NOT DEFINED CDDP_BINARY_DIR OR NOT DEFINED CDDP_SOURCE_DIR) + message(FATAL_ERROR "CDDP_BINARY_DIR and CDDP_SOURCE_DIR must be provided") +endif() + +set(SMOKE_ROOT "${CDDP_BINARY_DIR}/package_smoke") +set(SMOKE_PREFIX "${SMOKE_ROOT}/install") +set(CONSUMER_SOURCE_DIR "${CDDP_SOURCE_DIR}/tests/package_smoke") +set(CONSUMER_BUILD_DIR "${SMOKE_ROOT}/consumer-build") + +file(REMOVE_RECURSE "${SMOKE_ROOT}") +file(MAKE_DIRECTORY "${SMOKE_ROOT}") + +execute_process( + COMMAND "${CMAKE_COMMAND}" --install "${CDDP_BINARY_DIR}" --prefix "${SMOKE_PREFIX}" + RESULT_VARIABLE INSTALL_RESULT + OUTPUT_VARIABLE INSTALL_STDOUT + ERROR_VARIABLE INSTALL_STDERR +) +if(NOT INSTALL_RESULT EQUAL 0) + message(FATAL_ERROR "PackageInstallSmoke install failed:\n${INSTALL_STDOUT}\n${INSTALL_STDERR}") +endif() + +execute_process( + COMMAND "${CMAKE_COMMAND}" -S "${CONSUMER_SOURCE_DIR}" -B "${CONSUMER_BUILD_DIR}" + -DCMAKE_BUILD_TYPE=Release + "-DCMAKE_PREFIX_PATH=${SMOKE_PREFIX}" + RESULT_VARIABLE CONFIGURE_RESULT + OUTPUT_VARIABLE CONFIGURE_STDOUT + ERROR_VARIABLE CONFIGURE_STDERR +) +if(NOT CONFIGURE_RESULT EQUAL 0) + message(FATAL_ERROR "PackageInstallSmoke configure failed:\n${CONFIGURE_STDOUT}\n${CONFIGURE_STDERR}") +endif() + +execute_process( + COMMAND "${CMAKE_COMMAND}" --build "${CONSUMER_BUILD_DIR}" --parallel 4 + RESULT_VARIABLE BUILD_RESULT + OUTPUT_VARIABLE BUILD_STDOUT + ERROR_VARIABLE BUILD_STDERR +) +if(NOT BUILD_RESULT EQUAL 0) + message(FATAL_ERROR "PackageInstallSmoke build failed:\n${BUILD_STDOUT}\n${BUILD_STDERR}") +endif() diff --git a/tests/test_matplot.cpp b/tests/test_matplot.cpp deleted file mode 100644 index 566a6991..00000000 --- a/tests/test_matplot.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include -#ifdef CDDP_HAS_MATPLOT -#include -#endif -#include - -TEST(MatplotTest, BasicPlot) { - using namespace matplot; - auto [X, Y] = meshgrid(linspace(-5, +5, 40), linspace(-5, +5, 40)); - auto Z = transform(X, Y, [](double x, double y) { - return 10 * 2 + pow(x, 2) - 10 * cos(2 * pi * x) + pow(y, 2) - - 10 * cos(2 * pi * y); - }); - surf(X, Y, Z); -} - -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 69c15b2fc7081971184ccc85616afbd48bc35e4a Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 00:35:41 -0400 Subject: [PATCH 04/14] Fix Python solver validation and callback errors This raises Python-facing errors for invalid solver names and initial trajectories while preserving the existing C++ API behavior. It also releases the GIL around solve(), rethrows threaded callback failures, and adds regression tests for nonlinear objectives and solution fields. --- README.md | 2 +- python/src/bind_dynamics.cpp | 1 + python/src/bind_options.cpp | 6 +- python/src/bind_solver.cpp | 112 +++++++++++++++++-- python/tests/test_custom_dynamics.py | 48 ++++++++ python/tests/test_nonlinear_objective.py | 58 ++++++++++ python/tests/test_pendulum.py | 34 +++++- python/tests/test_solver_errors.py | 65 +++++++++++ src/cddp_core/cddp_solver_base.cpp | 8 ++ tests/dynamics_model/test_spacecraft_roe.cpp | 50 --------- 10 files changed, 317 insertions(+), 67 deletions(-) create mode 100644 python/tests/test_nonlinear_objective.py create mode 100644 python/tests/test_solver_errors.py diff --git a/README.md b/README.md index 5833af92..44201db5 100644 --- a/README.md +++ b/README.md @@ -55,7 +55,7 @@ brew install eigen # For macOS ### Building ```bash -git clone -b v0.3.2 https://github.com/astomodynamics/cddp-cpp +git clone https://github.com/astomodynamics/cddp-cpp cd cddp-cpp mkdir build && cd build cmake .. diff --git a/python/src/bind_dynamics.cpp b/python/src/bind_dynamics.cpp index 4d2caf74..cd4124a0 100644 --- a/python/src/bind_dynamics.cpp +++ b/python/src/bind_dynamics.cpp @@ -36,6 +36,7 @@ class PyDynamicalSystem : public cddp::DynamicalSystem { Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const override { + // Map C++ virtual dispatch to the snake_case Python API exported below. PYBIND11_OVERRIDE_NAME(Eigen::VectorXd, cddp::DynamicalSystem, "get_continuous_dynamics", getContinuousDynamics, state, control, time); diff --git a/python/src/bind_options.cpp b/python/src/bind_options.cpp index a0f2e1a3..747e70ab 100644 --- a/python/src/bind_options.cpp +++ b/python/src/bind_options.cpp @@ -74,7 +74,7 @@ void bind_options(py::module_& m) { .def_readwrite("slack_var_init_scale", &cddp::InteriorPointOptions::slack_var_init_scale) .def_readwrite("barrier", &cddp::InteriorPointOptions::barrier); - // LogBarrierOptions (inherits MultiShootingOptions) + // LogBarrierOptions exposed as a flat Python binding; no Python base class. py::class_(m, "LogBarrierOptions") .def(py::init<>()) .def_readwrite("segment_length", &cddp::LogBarrierOptions::segment_length) @@ -85,14 +85,14 @@ void bind_options(py::module_& m) { .def_readwrite("relaxed_log_barrier_delta", &cddp::LogBarrierOptions::relaxed_log_barrier_delta) .def_readwrite("barrier", &cddp::LogBarrierOptions::barrier); - // IPDDPAlgorithmOptions (inherits InteriorPointOptions) + // IPDDPAlgorithmOptions exposed as a flat Python binding. py::class_(m, "IPDDPOptions") .def(py::init<>()) .def_readwrite("dual_var_init_scale", &cddp::IPDDPAlgorithmOptions::dual_var_init_scale) .def_readwrite("slack_var_init_scale", &cddp::IPDDPAlgorithmOptions::slack_var_init_scale) .def_readwrite("barrier", &cddp::IPDDPAlgorithmOptions::barrier); - // MSIPDDPAlgorithmOptions (inherits InteriorPointOptions + MultiShootingOptions) + // MSIPDDPAlgorithmOptions exposed as a flat Python binding. py::class_(m, "MSIPDDPOptions") .def(py::init<>()) .def_readwrite("dual_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::dual_var_init_scale) diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index ee26f134..5621cf1e 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -1,3 +1,6 @@ +#include +#include + #include #include #include @@ -8,6 +11,82 @@ namespace py = pybind11; namespace { +constexpr std::array kBuiltinSolverNames = { + "CLDDP", "LogDDP", "IPDDP", "MSIPDDP"}; + +bool isBuiltinSolverName(const std::string &solver_name) { + for (const char *name : kBuiltinSolverNames) { + if (solver_name == name) { + return true; + } + } + return false; +} + +std::string builtinSolverList() { + std::ostringstream stream; + for (std::size_t i = 0; i < kBuiltinSolverNames.size(); ++i) { + if (i > 0) { + stream << ", "; + } + stream << kBuiltinSolverNames[i]; + } + return stream.str(); +} + +void validateInitialTrajectory(cddp::CDDP &solver, + const std::vector &X, + const std::vector &U) { + int state_dim = 0; + int control_dim = 0; + try { + state_dim = solver.getStateDim(); + control_dim = solver.getControlDim(); + } catch (const std::exception &) { + throw py::value_error( + "set_initial_trajectory requires a dynamical system to be set first."); + } + + const std::size_t expected_state_count = + static_cast(solver.getHorizon() + 1); + const std::size_t expected_control_count = + static_cast(solver.getHorizon()); + + if (X.size() != expected_state_count || U.size() != expected_control_count) { + std::ostringstream stream; + stream << "set_initial_trajectory expected X length " + << expected_state_count << " and U length " + << expected_control_count << ", got X length " << X.size() + << " and U length " << U.size() << "."; + throw py::value_error(stream.str()); + } + + for (std::size_t i = 0; i < X.size(); ++i) { + if (X[i].size() != state_dim) { + std::ostringstream stream; + stream << "set_initial_trajectory expected state vector " << i + << " to have dimension " << state_dim << ", got " + << X[i].size() << "."; + throw py::value_error(stream.str()); + } + } + + for (std::size_t i = 0; i < U.size(); ++i) { + if (U[i].size() != control_dim) { + std::ostringstream stream; + stream << "set_initial_trajectory expected control vector " << i + << " to have dimension " << control_dim << ", got " + << U[i].size() << "."; + throw py::value_error(stream.str()); + } + } +} + +// These wrappers let the C++ solver take ownership of Python-defined callbacks +// without adopting the Python object's raw allocation. The py::object keeps the +// original Python instance alive, the raw pointer only forwards virtual calls, +// and each callback reacquires the GIL because solver work may run on worker +// threads when parallel execution is enabled. class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { public: PythonBackedDynamicalSystem(py::object owner, cddp::DynamicalSystem *wrapped) @@ -345,7 +424,8 @@ void bind_solver(py::module_ &m) { std::move(system), wrapped)); }, py::arg("system"), - "Keep a Python-owned dynamics model alive inside the solver.") + "Transfer ownership of a Python dynamics model to the solver. " + "Do not reuse the Python object after this call.") .def("set_objective", [](cddp::CDDP &self, py::object objective) { auto *wrapped = objective.cast(); @@ -353,7 +433,8 @@ void bind_solver(py::module_ &m) { std::move(objective), wrapped)); }, py::arg("objective"), - "Keep a Python-owned objective alive inside the solver.") + "Transfer ownership of a Python objective to the solver. Do not " + "reuse the Python object after this call.") .def("add_constraint", [](cddp::CDDP &self, const std::string &name, py::object constraint) { auto *wrapped = constraint.cast(); @@ -362,7 +443,8 @@ void bind_solver(py::module_ &m) { std::move(constraint), wrapped)); }, py::arg("name"), py::arg("constraint"), - "Keep a Python-owned path constraint alive inside the solver.") + "Transfer ownership of a Python path constraint to the solver. " + "Do not reuse the Python object after this call.") .def("add_terminal_constraint", [](cddp::CDDP &self, const std::string &name, py::object constraint) { auto *wrapped = constraint.cast(); @@ -371,17 +453,33 @@ void bind_solver(py::module_ &m) { std::move(constraint), wrapped)); }, py::arg("name"), py::arg("constraint"), - "Keep a Python-owned terminal constraint alive inside the solver.") + "Transfer ownership of a Python terminal constraint to the " + "solver. Do not reuse the Python object after this call.") .def("remove_constraint", &cddp::CDDP::removePathConstraint, py::arg("name")) .def("remove_terminal_constraint", &cddp::CDDP::removeTerminalConstraint, py::arg("name")) - .def("set_initial_trajectory", &cddp::CDDP::setInitialTrajectory, + .def("set_initial_trajectory", + [](cddp::CDDP &self, const std::vector &X, + const std::vector &U) { + validateInitialTrajectory(self, X, U); + self.setInitialTrajectory(X, U); + }, py::arg("X"), py::arg("U")) .def("solve", py::overload_cast(&cddp::CDDP::solve), - py::arg("solver_type") = cddp::SolverType::CLDDP) + py::arg("solver_type") = cddp::SolverType::CLDDP, + py::call_guard()) .def("solve_by_name", - py::overload_cast(&cddp::CDDP::solve), + [](cddp::CDDP &self, const std::string &solver_type) { + if (!isBuiltinSolverName(solver_type)) { + throw py::value_error( + "Unknown solver '" + solver_type + + "'. Supported solver names: " + builtinSolverList() + + "."); + } + py::gil_scoped_release release; + return self.solve(solver_type); + }, py::arg("solver_type")) .def_property_readonly("initial_state", &cddp::CDDP::getInitialState) .def_property_readonly("reference_state", diff --git a/python/tests/test_custom_dynamics.py b/python/tests/test_custom_dynamics.py index 4c58ff72..a715d27c 100644 --- a/python/tests/test_custom_dynamics.py +++ b/python/tests/test_custom_dynamics.py @@ -1,5 +1,7 @@ """Test custom dynamics defined in Python via the trampoline.""" import numpy as np +import pytest +import threading import pycddp @@ -27,6 +29,19 @@ def get_cross_hessian(self, state, control, time=0.0): return [np.zeros((1, 2)), np.zeros((1, 2))] +class ExplodingDoubleIntegrator(DoubleIntegrator): + def __init__(self, dt): + super().__init__(dt) + self._main_thread_id = threading.get_ident() + + def get_discrete_dynamics(self, state, control, time=0.0): + if threading.get_ident() != self._main_thread_id: + raise RuntimeError("boom from Python dynamics") + return state + self.timestep * self.get_continuous_dynamics( + state, control, time + ) + + def test_custom_dynamics_continuous(): sys = DoubleIntegrator(0.1) assert sys.state_dim == 2 @@ -74,7 +89,40 @@ def test_custom_dynamics_with_solver(): solution = solver.solve(pycddp.SolverType.CLDDP) + assert solution.solver_name == "CLDDP" + assert solution.status_message assert solution.iterations_completed >= 0 + assert len(solution.time_points) == horizon + 1 assert len(solution.state_trajectory) == horizon + 1 assert len(solution.control_trajectory) == horizon + assert len(solution.feedback_gains) == horizon + assert np.isfinite(solution.final_objective) + assert np.isfinite(solution.final_step_length) + assert np.isfinite(solution.final_regularization) assert solution.solve_time_ms >= 0 + + +def test_parallel_python_callback_exceptions_surface_to_python(): + dt = 0.1 + horizon = 60 + + opts = pycddp.CDDPOptions() + opts.max_iterations = 2 + opts.verbose = False + opts.print_solver_header = False + opts.enable_parallel = True + opts.num_threads = 2 + + solver = pycddp.CDDP(np.array([1.0, 0.0]), np.zeros(2), horizon, dt, opts) + solver.set_dynamical_system(ExplodingDoubleIntegrator(dt)) + solver.set_objective( + pycddp.QuadraticObjective( + np.eye(2), 0.1 * np.eye(1), 10.0 * np.eye(2), np.zeros(2), [], dt + ) + ) + solver.add_constraint( + "ctrl", pycddp.ControlConstraint(np.array([-5.0]), np.array([5.0])) + ) + + with pytest.raises(RuntimeError, match="boom from Python dynamics"): + solver.solve(pycddp.SolverType.LogDDP) diff --git a/python/tests/test_nonlinear_objective.py b/python/tests/test_nonlinear_objective.py new file mode 100644 index 00000000..8a15bfca --- /dev/null +++ b/python/tests/test_nonlinear_objective.py @@ -0,0 +1,58 @@ +"""Regression tests for Python-defined nonlinear objectives.""" +import numpy as np + +import pycddp + + +class CountingNonlinearObjective(pycddp.NonlinearObjective): + def __init__(self, timestep, counters): + super().__init__(timestep) + self._counters = counters + + def evaluate(self, states, controls): + self._counters["evaluate"] += 1 + total = 0.0 + for state, control in zip(states[:-1], controls): + total += self.running_cost(state, control, 0) + total += self.terminal_cost(states[-1]) + return total + + def running_cost(self, state, control, index): + self._counters["running_cost"] += 1 + return float(state @ state + 0.1 * control @ control) + + def terminal_cost(self, final_state): + self._counters["terminal_cost"] += 1 + return float(10.0 * final_state @ final_state) + + +def test_python_nonlinear_objective_dispatches_through_solver(): + dt = 0.1 + horizon = 15 + counters = {"evaluate": 0, "running_cost": 0, "terminal_cost": 0} + + opts = pycddp.CDDPOptions() + opts.max_iterations = 20 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(np.array([1.0, 0.0]), np.zeros(2), horizon, dt, opts) + solver.set_dynamical_system( + pycddp.LTISystem( + np.array([[0.0, 1.0], [0.0, 0.0]]), + np.array([[0.0], [1.0]]), + dt, + ) + ) + solver.set_objective(CountingNonlinearObjective(dt, counters)) + solver.add_constraint( + "ctrl", pycddp.ControlConstraint(np.array([-2.0]), np.array([2.0])) + ) + + solution = solver.solve(pycddp.SolverType.LogDDP) + + assert solution.solver_name == "LogDDP" + assert solution.status_message + assert counters["evaluate"] > 0 + assert counters["running_cost"] > 0 + assert counters["terminal_cost"] > 0 diff --git a/python/tests/test_pendulum.py b/python/tests/test_pendulum.py index 1d681b8e..4ea94807 100644 --- a/python/tests/test_pendulum.py +++ b/python/tests/test_pendulum.py @@ -3,6 +3,20 @@ import pycddp +def _assert_common_solution_fields(solution, horizon, solver_name): + assert solution.solver_name == solver_name + assert solution.status_message + assert solution.iterations_completed > 0 + assert solution.solve_time_ms >= 0 + assert np.isfinite(solution.final_objective) + assert np.isfinite(solution.final_step_length) + assert np.isfinite(solution.final_regularization) + assert len(solution.time_points) == horizon + 1 + assert len(solution.state_trajectory) == horizon + 1 + assert len(solution.control_trajectory) == horizon + assert len(solution.feedback_gains) == horizon + + def test_pendulum_swing_up(): dt = 0.05 horizon = 50 @@ -25,10 +39,8 @@ def test_pendulum_swing_up(): solution = solver.solve(pycddp.SolverType.CLDDP) - assert solution.iterations_completed > 0 - assert len(solution.state_trajectory) == horizon + 1 - assert len(solution.control_trajectory) == horizon - assert solution.solve_time_ms >= 0 + _assert_common_solution_fields(solution, horizon, "CLDDP") + assert np.linalg.norm(solution.state_trajectory[-1] - xref) < np.linalg.norm(x0 - xref) def test_pendulum_logddp(): @@ -45,6 +57,7 @@ def test_pendulum_logddp(): opts.max_iterations = 100 opts.verbose = False opts.print_solver_header = False + opts.return_iteration_info = True solver = pycddp.CDDP(x0, xref, horizon, dt, opts) solver.set_dynamical_system(pycddp.Pendulum(dt, length=0.5, mass=1.0)) @@ -52,5 +65,14 @@ def test_pendulum_logddp(): solver.add_constraint("ctrl", pycddp.ControlConstraint(np.array([-50.0]), np.array([50.0]))) solution = solver.solve(pycddp.SolverType.LogDDP) - assert solution.iterations_completed > 0 - assert len(solution.state_trajectory) == horizon + 1 + _assert_common_solution_fields(solution, horizon, "LogDDP") + history = solution.history + assert len(history.objective) >= 1 + assert len(history.objective) == len(history.merit_function) + assert len(history.objective) == len(history.step_length_primal) + assert len(history.objective) == len(history.step_length_dual) + assert len(history.objective) == len(history.dual_infeasibility) + assert len(history.objective) == len(history.primal_infeasibility) + assert len(history.objective) == len(history.complementary_infeasibility) + assert len(history.objective) == len(history.regularization) + assert len(history.objective) == len(history.barrier_mu) diff --git a/python/tests/test_solver_errors.py b/python/tests/test_solver_errors.py new file mode 100644 index 00000000..79afd979 --- /dev/null +++ b/python/tests/test_solver_errors.py @@ -0,0 +1,65 @@ +"""Regression tests for Python-facing solver validation.""" +import numpy as np +import pytest + +import pycddp + + +def _make_solver(horizon=6, dt=0.1): + x0 = np.array([0.0, 0.0]) + xref = np.array([0.0, 0.0]) + opts = pycddp.CDDPOptions() + opts.verbose = False + opts.print_solver_header = False + return pycddp.CDDP(x0, xref, horizon, dt, opts) + + +def test_solve_by_name_raises_for_unknown_solver(): + solver = _make_solver() + + with pytest.raises(ValueError, match="Unknown solver 'NONEXISTENT'"): + solver.solve_by_name("NONEXISTENT") + + +def test_set_initial_trajectory_requires_dynamical_system(): + solver = _make_solver() + X = [np.zeros(2) for _ in range(solver.horizon + 1)] + U = [np.zeros(1) for _ in range(solver.horizon)] + + with pytest.raises(ValueError, match="requires a dynamical system"): + solver.set_initial_trajectory(X, U) + + +def test_set_initial_trajectory_rejects_bad_lengths(): + solver = _make_solver() + solver.set_dynamical_system(pycddp.Pendulum(0.1)) + + X = [np.zeros(2) for _ in range(solver.horizon)] + U = [np.zeros(1) for _ in range(solver.horizon)] + + with pytest.raises(ValueError, match="expected X length"): + solver.set_initial_trajectory(X, U) + + +def test_set_initial_trajectory_rejects_bad_state_dimension(): + solver = _make_solver() + solver.set_dynamical_system(pycddp.Pendulum(0.1)) + + X = [np.zeros(2) for _ in range(solver.horizon + 1)] + X[2] = np.zeros(3) + U = [np.zeros(1) for _ in range(solver.horizon)] + + with pytest.raises(ValueError, match="state vector 2"): + solver.set_initial_trajectory(X, U) + + +def test_set_initial_trajectory_rejects_bad_control_dimension(): + solver = _make_solver() + solver.set_dynamical_system(pycddp.Pendulum(0.1)) + + X = [np.zeros(2) for _ in range(solver.horizon + 1)] + U = [np.zeros(1) for _ in range(solver.horizon)] + U[1] = np.zeros(2) + + with pytest.raises(ValueError, match="control vector 1"): + solver.set_initial_trajectory(X, U) diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index f3438ed0..db039607 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -17,6 +17,7 @@ #include "cddp_core/cddp_solver_base.hpp" #include #include +#include #include #include #include @@ -264,6 +265,7 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { // Multi-threaded std::vector> futures; futures.reserve(context.alphas_.size()); + std::exception_ptr first_exception; for (double alpha : context.alphas_) { futures.push_back( @@ -285,6 +287,9 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { } catch (const std::exception &e) { ++failed_count; last_error = e.what(); + if (!first_exception) { + first_exception = std::current_exception(); + } if (options.verbose) { std::cerr << getSolverName() << ": Forward pass thread failed: " << e.what() @@ -298,6 +303,9 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { << ": ALL forward pass threads failed. Last error: " << last_error << std::endl; } + if (first_exception) { + std::rethrow_exception(first_exception); + } } return best_result; diff --git a/tests/dynamics_model/test_spacecraft_roe.cpp b/tests/dynamics_model/test_spacecraft_roe.cpp index 8b72f3ff..2139000b 100644 --- a/tests/dynamics_model/test_spacecraft_roe.cpp +++ b/tests/dynamics_model/test_spacecraft_roe.cpp @@ -145,56 +145,6 @@ TEST(TestSpacecraftROE, RelativeTrajectory) ASSERT_GT(hcw_trajectory.back().norm(), 1e-3); - // 7) Plotting - namespace plt = matplot; - std::vector t_states_plot(num_steps + 1); - for(int i = 0; i <= num_steps; ++i) t_states_plot[i] = i * dt; - - std::vector x_pos_plot(num_steps + 1), y_pos_plot(num_steps + 1), z_pos_plot(num_steps + 1); - for (size_t i = 0; i < hcw_trajectory.size(); ++i) { - const auto& state_hcw = hcw_trajectory[i]; - x_pos_plot[i] = state_hcw(0); - y_pos_plot[i] = state_hcw(1); - z_pos_plot[i] = state_hcw(2); - } - - // // X-Y plane trajectory - // plt::figure(); - // plt::plot(y_pos_plot, x_pos_plot)->line_width(2).display_name("Trajectory"); - // plt::hold(true); - // if (!x_pos_plot.empty() && !y_pos_plot.empty()){ - // plt::scatter(std::vector{y_pos_plot.front()}, std::vector{x_pos_plot.front()}) - // ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - // plt::scatter(std::vector{y_pos_plot.back()}, std::vector{x_pos_plot.back()}) - // ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - // } - // plt::hold(false); - // plt::xlabel("y (m) [In-track]"); - // plt::ylabel("x (m) [Radial]"); - // plt::legend(); - // plt::title("HCW X-Y Plane Trajectory (from ROE propagation)"); - // plt::axis(plt::equal); - // plt::gca()->x_axis().reverse(true); - - // // 3D Trajectory - // plt::figure(); - // plt::plot3(x_pos_plot, y_pos_plot, z_pos_plot, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - // plt::hold(true); - // if (!x_pos_plot.empty()){ - // plt::scatter3(std::vector{x_pos_plot.front()}, std::vector{y_pos_plot.front()}, std::vector{z_pos_plot.front()}) - // ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - // plt::scatter3(std::vector{x_pos_plot.back()}, std::vector{y_pos_plot.back()}, std::vector{z_pos_plot.back()}) - // ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - // } - // plt::hold(false); - // plt::xlabel("x (m) [Radial]"); - // plt::ylabel("y (m) [In-track]"); - // plt::zlabel("z (m) [Cross-track]"); - // plt::legend(); - // plt::title("3D HCW Trajectory (from ROE propagation)"); - // plt::axis(plt::equal); - - // plt::show(); // Show all plots } TEST(TestSpacecraftROE, JacobianBasedPropagation) From d2bc543194665969fba4a29f542ee6fd2f657735 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 01:12:11 -0400 Subject: [PATCH 05/14] Fix Python constraint trampolines and line search fallback This restores parallel forward-pass behavior so a successful alpha still wins when another trial throws. It also adds a Python Constraint trampoline plus regression coverage so custom path constraints can be defined and dispatched through the solver. --- python/src/bind_constraints.cpp | 98 +++++++++++++++++++++++++++++- python/tests/test_constraints.py | 74 ++++++++++++++++++++++ src/cddp_core/cddp_solver_base.cpp | 2 +- tests/cddp_core/test_cddp_core.cpp | 63 ++++++++++++++++++- 4 files changed, 234 insertions(+), 3 deletions(-) diff --git a/python/src/bind_constraints.cpp b/python/src/bind_constraints.cpp index 6dbe68e1..d1ac3c0b 100644 --- a/python/src/bind_constraints.cpp +++ b/python/src/bind_constraints.cpp @@ -6,8 +6,95 @@ namespace py = pybind11; +class PyConstraint : public cddp::Constraint { +public: + using cddp::Constraint::Constraint; + + int getDualDim() const override { + PYBIND11_OVERRIDE_NAME(int, cddp::Constraint, "get_dual_dim", getDualDim); + } + + Eigen::VectorXd evaluate(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_PURE_NAME(Eigen::VectorXd, cddp::Constraint, "evaluate", + evaluate, state, control, index); + } + + Eigen::VectorXd getLowerBound() const override { + PYBIND11_OVERRIDE_PURE_NAME(Eigen::VectorXd, cddp::Constraint, + "get_lower_bound", getLowerBound); + } + + Eigen::VectorXd getUpperBound() const override { + PYBIND11_OVERRIDE_PURE_NAME(Eigen::VectorXd, cddp::Constraint, + "get_upper_bound", getUpperBound); + } + + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_PURE_NAME(Eigen::MatrixXd, cddp::Constraint, + "get_state_jacobian", getStateJacobian, + state, control, index); + } + + Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_PURE_NAME(Eigen::MatrixXd, cddp::Constraint, + "get_control_jacobian", getControlJacobian, + state, control, index); + } + + double computeViolation(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_PURE_NAME(double, cddp::Constraint, + "compute_violation", computeViolation, state, + control, index); + } + + double computeViolationFromValue(const Eigen::VectorXd &g) const override { + PYBIND11_OVERRIDE_PURE_NAME(double, cddp::Constraint, + "compute_violation_from_value", + computeViolationFromValue, g); + } + + Eigen::VectorXd getCenter() const override { + PYBIND11_OVERRIDE_NAME(Eigen::VectorXd, cddp::Constraint, "get_center", + getCenter); + } + + std::vector getStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_NAME(std::vector, cddp::Constraint, + "get_state_hessian", getStateHessian, state, + control, index); + } + + std::vector + getControlHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_NAME(std::vector, cddp::Constraint, + "get_control_hessian", getControlHessian, state, + control, index); + } + + std::vector getCrossHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + PYBIND11_OVERRIDE_NAME(std::vector, cddp::Constraint, + "get_cross_hessian", getCrossHessian, state, + control, index); + } +}; + void bind_constraints(py::module_& m) { - py::class_(m, "Constraint") + py::class_(m, "Constraint") + .def(py::init(), py::arg("name")) .def("evaluate", &cddp::Constraint::evaluate, py::arg("state"), py::arg("control"), py::arg("index") = 0) .def("get_lower_bound", &cddp::Constraint::getLowerBound) @@ -18,6 +105,15 @@ void bind_constraints(py::module_& m) { py::arg("state"), py::arg("control"), py::arg("index") = 0) .def("compute_violation", &cddp::Constraint::computeViolation, py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("compute_violation_from_value", + &cddp::Constraint::computeViolationFromValue, py::arg("g")) + .def("get_center", &cddp::Constraint::getCenter) + .def("get_state_hessian", &cddp::Constraint::getStateHessian, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("get_control_hessian", &cddp::Constraint::getControlHessian, + py::arg("state"), py::arg("control"), py::arg("index") = 0) + .def("get_cross_hessian", &cddp::Constraint::getCrossHessian, + py::arg("state"), py::arg("control"), py::arg("index") = 0) .def("get_dual_dim", &cddp::Constraint::getDualDim) .def_property_readonly("name", &cddp::Constraint::getName); diff --git a/python/tests/test_constraints.py b/python/tests/test_constraints.py index 834b9b72..ab30ed35 100644 --- a/python/tests/test_constraints.py +++ b/python/tests/test_constraints.py @@ -3,6 +3,41 @@ import pycddp +class CountingAffineConstraint(pycddp.Constraint): + def __init__(self, counters): + super().__init__("CountingAffineConstraint") + self._counters = counters + + def get_dual_dim(self): + return 1 + + def evaluate(self, state, control, index=0): + self._counters["evaluate"] += 1 + return np.array([state[0] - 10.0]) + + def get_lower_bound(self): + return np.array([-np.inf]) + + def get_upper_bound(self): + return np.array([0.0]) + + def get_state_jacobian(self, state, control, index=0): + self._counters["state_jacobian"] += 1 + return np.array([[1.0, 0.0]]) + + def get_control_jacobian(self, state, control, index=0): + self._counters["control_jacobian"] += 1 + return np.array([[0.0]]) + + def compute_violation(self, state, control, index=0): + self._counters["compute_violation"] += 1 + return max(0.0, float(self.evaluate(state, control, index)[0])) + + def compute_violation_from_value(self, g): + self._counters["compute_violation_from_value"] += 1 + return max(0.0, float(g[0])) + + def test_control_constraint(): c = pycddp.ControlConstraint(np.array([-1.0, -2.0]), np.array([1.0, 2.0])) assert c.get_dual_dim() == 4 # 2 lower + 2 upper bounds @@ -37,3 +72,42 @@ def test_linear_constraint(): control = np.array([0.0]) val = c.evaluate(state, control) assert val.shape[0] == 2 + + +def test_custom_python_constraint_with_solver(): + counters = { + "evaluate": 0, + "state_jacobian": 0, + "control_jacobian": 0, + "compute_violation": 0, + "compute_violation_from_value": 0, + } + + dt = 0.05 + horizon = 20 + x0 = np.array([np.pi, 0.0]) + xref = np.array([0.0, 0.0]) + + opts = pycddp.CDDPOptions() + opts.max_iterations = 10 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system( + pycddp.Pendulum(dt, length=0.5, mass=1.0, damping=0.01) + ) + solver.set_objective( + pycddp.QuadraticObjective( + np.zeros((2, 2)), 0.1 * np.eye(1), 100.0 * np.eye(2), xref, [], dt + ) + ) + solver.add_constraint("custom", CountingAffineConstraint(counters)) + + solution = solver.solve(pycddp.SolverType.LogDDP) + + assert solution.solver_name == "LogDDP" + assert solution.status_message + assert counters["evaluate"] > 0 + assert counters["state_jacobian"] > 0 + assert counters["control_jacobian"] > 0 diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index db039607..a3ecf407 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -303,7 +303,7 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { << ": ALL forward pass threads failed. Last error: " << last_error << std::endl; } - if (first_exception) { + if (first_exception && !best_result.success) { std::rethrow_exception(first_exception); } } diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index ce3f8a18..3ae7db33 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -110,6 +110,44 @@ class AnotherMockSolver : public ISolverAlgorithm { } }; +class ThrowingLineSearchSolver : public CDDPSolverBase { +public: + void initialize(CDDP &context) override {} + + ForwardPassResult runPerformForwardPass(CDDP &context) { + return performForwardPass(context); + } + + std::string getSolverName() const override { + return "ThrowingLineSearchSolver"; + } + +protected: + bool backwardPass(CDDP &context) override { + return true; + } + + ForwardPassResult forwardPass(CDDP &context, double alpha) override { + if (alpha >= 1.0) { + throw std::runtime_error("alpha trial failed"); + } + + ForwardPassResult result; + result.alpha_pr = alpha; + result.cost = alpha; + result.merit_function = alpha; + result.success = (alpha == 0.5); + return result; + } + + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override { + return false; + } + + void printIteration(int iter, const CDDP &context) const override {} +}; + // Factory functions for the mock solvers std::unique_ptr createMockExternalSolver() { return std::make_unique(); @@ -268,6 +306,29 @@ TEST_F(CDDPCoreTest, UnknownSolverErrorHandling) { EXPECT_EQ(solution.iterations_completed, 0); } +TEST_F(CDDPCoreTest, ParallelForwardPassKeepsSuccessfulAlphaWhenAnotherThrows) { + options.enable_parallel = true; + + cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep, + std::make_unique(timestep, "euler"), + std::make_unique( + Eigen::MatrixXd::Identity(state_dim, state_dim), + Eigen::MatrixXd::Identity(control_dim, control_dim), + 10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim), + goal_state, std::vector(), timestep), + options); + + cddp_solver.alphas_ = {1.0, 0.5, 0.25}; + + cddp::ThrowingLineSearchSolver solver; + cddp::ForwardPassResult result; + + EXPECT_NO_THROW(result = solver.runPerformForwardPass(cddp_solver)); + EXPECT_TRUE(result.success); + EXPECT_DOUBLE_EQ(result.alpha_pr, 0.5); + EXPECT_DOUBLE_EQ(result.merit_function, 0.5); +} + // Test solver precedence (external over built-in) TEST_F(CDDPCoreTest, SolverPrecedence) { // Register a solver with the same name as a built-in solver @@ -351,4 +412,4 @@ TEST_F(CDDPCoreTest, IntegrationWithTrajectoryAndOptions) { 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 +} From a941218394b19004e8cb8e6ea0b46496ac37668a Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 01:54:14 -0400 Subject: [PATCH 06/14] Fix Python diagnostics and clean dead Matplot tests Improve Python import/autodiff error reporting and clarify solver ownership docs. Handle parallel derivative precompute exceptions after draining worker futures, and delete unreachable Matplot test code instead of preserving dead guards. --- python/pycddp/__init__.py | 22 +- python/src/bind_dynamics.cpp | 11 + python/src/bind_solver.cpp | 28 ++- python/tests/test_custom_dynamics.py | 32 +++ python/tests/test_solver_errors.py | 55 +++++ src/cddp_core/cddp_solver_base.cpp | 19 +- tests/cddp_core/test_cddp_core.cpp | 80 ++++++ tests/cddp_core/test_clddp_solver.cpp | 4 - tests/cddp_core/test_constraint.cpp | 226 +---------------- tests/cddp_core/test_ipddp_solver.cpp | 4 - tests/cddp_core/test_logddp_solver.cpp | 4 - tests/cddp_core/test_msipddp_core.cpp | 7 - tests/cddp_core/test_msipddp_solver.cpp | 4 - .../dynamics_model/test_attitude_dynamics.cpp | 216 ++--------------- tests/dynamics_model/test_mrp_attitude.cpp | 11 +- .../dynamics_model/test_spacecraft_linear.cpp | 69 +----- .../test_spacecraft_linear_fuel.cpp | 43 +--- .../test_spacecraft_nonlinear.cpp | 229 +----------------- tests/dynamics_model/test_spacecraft_roe.cpp | 3 - 19 files changed, 254 insertions(+), 813 deletions(-) diff --git a/python/pycddp/__init__.py b/python/pycddp/__init__.py index ff3b6cdc..9ea7ddd4 100644 --- a/python/pycddp/__init__.py +++ b/python/pycddp/__init__.py @@ -1,6 +1,14 @@ -"""pycddp - Python bindings for CDDP trajectory optimization.""" +"""Python bindings for Constrained Differential Dynamic Programming. -from pycddp._pycddp_core import ( +Main entry points: +- `CDDP` for solver setup and execution +- `DynamicalSystem` for system models +- `Objective` for cost functions +- `Constraint` for path and terminal constraints +""" + +try: + from pycddp._pycddp_core import ( # Enums SolverType, BarrierStrategy, @@ -66,7 +74,15 @@ ThrustMagnitudeConstraint, MaxThrustMagnitudeConstraint, -) + ) +except ImportError as exc: + raise ImportError( + "Failed to import the native pycddp extension '_pycddp_core'. " + "This usually means the extension was built for a different Python " + "version or a required native runtime library is missing. Reinstall " + "pycddp with the active interpreter and verify your C++ runtime " + "dependencies." + ) from exc from pycddp._version import __version__ diff --git a/python/src/bind_dynamics.cpp b/python/src/bind_dynamics.cpp index cd4124a0..4efa7efd 100644 --- a/python/src/bind_dynamics.cpp +++ b/python/src/bind_dynamics.cpp @@ -33,6 +33,17 @@ class PyDynamicalSystem : public cddp::DynamicalSystem { public: using cddp::DynamicalSystem::DynamicalSystem; + cddp::VectorXdual2nd + getContinuousDynamicsAutodiff(const cddp::VectorXdual2nd &state, + const cddp::VectorXdual2nd &control, + double time) const override { + throw std::runtime_error( + "Python-defined DynamicalSystem objects do not support " + "getContinuousDynamicsAutodiff. Override get_state_jacobian, " + "get_control_jacobian, and any needed Hessian methods in Python, " + "or use a built-in C++ dynamics model."); + } + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const override { diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 5621cf1e..cd367db2 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -82,11 +82,11 @@ void validateInitialTrajectory(cddp::CDDP &solver, } } -// These wrappers let the C++ solver take ownership of Python-defined callbacks -// without adopting the Python object's raw allocation. The py::object keeps the +// These wrappers let the C++ solver hold Python-defined solver objects without +// adopting the Python object's raw allocation. The py::object keeps the // original Python instance alive, the raw pointer only forwards virtual calls, -// and each callback reacquires the GIL because solver work may run on worker -// threads when parallel execution is enabled. +// and each forwarded method reacquires the GIL because solver work may run on +// worker threads when parallel execution is enabled. class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { public: PythonBackedDynamicalSystem(py::object owner, cddp::DynamicalSystem *wrapped) @@ -424,8 +424,9 @@ void bind_solver(py::module_ &m) { std::move(system), wrapped)); }, py::arg("system"), - "Transfer ownership of a Python dynamics model to the solver. " - "Do not reuse the Python object after this call.") + "The solver keeps a Python reference that keeps this dynamics " + "object alive. Mutating or sharing the object after this call " + "may produce unexpected behavior.") .def("set_objective", [](cddp::CDDP &self, py::object objective) { auto *wrapped = objective.cast(); @@ -433,8 +434,9 @@ void bind_solver(py::module_ &m) { std::move(objective), wrapped)); }, py::arg("objective"), - "Transfer ownership of a Python objective to the solver. Do not " - "reuse the Python object after this call.") + "The solver keeps a Python reference that keeps this objective " + "alive. Mutating or sharing the object after this call may " + "produce unexpected behavior.") .def("add_constraint", [](cddp::CDDP &self, const std::string &name, py::object constraint) { auto *wrapped = constraint.cast(); @@ -443,8 +445,9 @@ void bind_solver(py::module_ &m) { std::move(constraint), wrapped)); }, py::arg("name"), py::arg("constraint"), - "Transfer ownership of a Python path constraint to the solver. " - "Do not reuse the Python object after this call.") + "The solver keeps a Python reference that keeps this path " + "constraint alive. Mutating or sharing the object after this " + "call may produce unexpected behavior.") .def("add_terminal_constraint", [](cddp::CDDP &self, const std::string &name, py::object constraint) { auto *wrapped = constraint.cast(); @@ -453,8 +456,9 @@ void bind_solver(py::module_ &m) { std::move(constraint), wrapped)); }, py::arg("name"), py::arg("constraint"), - "Transfer ownership of a Python terminal constraint to the " - "solver. Do not reuse the Python object after this call.") + "The solver keeps a Python reference that keeps this terminal " + "constraint alive. Mutating or sharing the object after this " + "call may produce unexpected behavior.") .def("remove_constraint", &cddp::CDDP::removePathConstraint, py::arg("name")) .def("remove_terminal_constraint", diff --git a/python/tests/test_custom_dynamics.py b/python/tests/test_custom_dynamics.py index a715d27c..1648e65e 100644 --- a/python/tests/test_custom_dynamics.py +++ b/python/tests/test_custom_dynamics.py @@ -42,6 +42,14 @@ def get_discrete_dynamics(self, state, control, time=0.0): ) +class MinimalDoubleIntegrator(pycddp.DynamicalSystem): + def __init__(self, dt): + super().__init__(2, 1, dt, "euler") + + def get_continuous_dynamics(self, state, control, time=0.0): + return np.array([state[1], control[0]]) + + def test_custom_dynamics_continuous(): sys = DoubleIntegrator(0.1) assert sys.state_dim == 2 @@ -126,3 +134,27 @@ def test_parallel_python_callback_exceptions_surface_to_python(): with pytest.raises(RuntimeError, match="boom from Python dynamics"): solver.solve(pycddp.SolverType.LogDDP) + + +def test_python_dynamics_autodiff_path_raises_clear_error(): + dt = 0.1 + horizon = 8 + + opts = pycddp.CDDPOptions() + opts.max_iterations = 2 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(np.array([1.0, 0.0]), np.zeros(2), horizon, dt, opts) + solver.set_dynamical_system(MinimalDoubleIntegrator(dt)) + solver.set_objective( + pycddp.QuadraticObjective( + np.eye(2), 0.1 * np.eye(1), 10.0 * np.eye(2), np.zeros(2), [], dt + ) + ) + solver.add_constraint( + "ctrl", pycddp.ControlConstraint(np.array([-5.0]), np.array([5.0])) + ) + + with pytest.raises(RuntimeError, match="do not support getContinuousDynamicsAutodiff"): + solver.solve(pycddp.SolverType.CLDDP) diff --git a/python/tests/test_solver_errors.py b/python/tests/test_solver_errors.py index 79afd979..a7c2cc95 100644 --- a/python/tests/test_solver_errors.py +++ b/python/tests/test_solver_errors.py @@ -1,6 +1,10 @@ """Regression tests for Python-facing solver validation.""" +import pathlib import numpy as np import pytest +import shutil +import subprocess +import sys import pycddp @@ -21,6 +25,37 @@ def test_solve_by_name_raises_for_unknown_solver(): solver.solve_by_name("NONEXISTENT") +def test_solve_by_name_accepts_builtin_solver(): + dt = 0.05 + horizon = 20 + x0 = np.array([np.pi, 0.0]) + xref = np.array([0.0, 0.0]) + + opts = pycddp.CDDPOptions() + opts.max_iterations = 20 + opts.verbose = False + opts.print_solver_header = False + + solver = pycddp.CDDP(x0, xref, horizon, dt, opts) + solver.set_dynamical_system( + pycddp.Pendulum(dt, length=0.5, mass=1.0, damping=0.01) + ) + solver.set_objective( + pycddp.QuadraticObjective( + np.zeros((2, 2)), 0.1 * np.eye(1), 100.0 * np.eye(2), xref, [], dt + ) + ) + solver.add_constraint( + "ctrl", pycddp.ControlConstraint(np.array([-50.0]), np.array([50.0])) + ) + + solution = solver.solve_by_name("CLDDP") + + assert solution.solver_name == "CLDDP" + assert solution.status_message + assert len(solution.state_trajectory) == horizon + 1 + + def test_set_initial_trajectory_requires_dynamical_system(): solver = _make_solver() X = [np.zeros(2) for _ in range(solver.horizon + 1)] @@ -63,3 +98,23 @@ def test_set_initial_trajectory_rejects_bad_control_dimension(): with pytest.raises(ValueError, match="control vector 1"): solver.set_initial_trajectory(X, U) + + +def test_import_error_message_is_actionable(tmp_path): + package_dir = tmp_path / "pycddp" + package_dir.mkdir() + + source_dir = pathlib.Path(__file__).resolve().parents[1] / "pycddp" + shutil.copy(source_dir / "__init__.py", package_dir / "__init__.py") + shutil.copy(source_dir / "_version.py", package_dir / "_version.py") + + proc = subprocess.run( + [sys.executable, "-c", "import pycddp"], + env={"PYTHONPATH": str(tmp_path)}, + capture_output=True, + text=True, + check=False, + ) + + assert proc.returncode != 0 + assert "Failed to import the native pycddp extension" in proc.stderr diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index a3ecf407..87126fbf 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -357,12 +357,29 @@ void CDDPSolverBase::precomputeDynamicsDerivatives( if (use_parallel) { std::vector> futures; futures.reserve(horizon); + std::exception_ptr first_exception; for (int t = 0; t < horizon; ++t) { futures.push_back( std::async(std::launch::async, compute_derivatives, t)); } for (auto &f : futures) { - f.get(); + try { + if (f.valid()) { + f.get(); + } + } catch (const std::exception &e) { + if (!first_exception) { + first_exception = std::current_exception(); + } + if (options.verbose) { + std::cerr << getSolverName() + << ": Dynamics derivatives computation thread failed: " + << e.what() << std::endl; + } + } + } + if (first_exception) { + std::rethrow_exception(first_exception); } } else { for (int t = 0; t < horizon; ++t) { diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index 3ae7db33..9aee9940 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -148,6 +148,61 @@ class ThrowingLineSearchSolver : public CDDPSolverBase { void printIteration(int iter, const CDDP &context) const override {} }; +class ThrowingJacobianSystem : public DynamicalSystem { +public: + explicit ThrowingJacobianSystem(double timestep) + : DynamicalSystem(2, 1, timestep, "euler") {} + + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + return Eigen::VectorXd::Zero(2); + } + + std::tuple + getJacobians(const Eigen::VectorXd &state, const Eigen::VectorXd &control, + double time) const override { + if (time >= getTimestep()) { + throw std::runtime_error("jacobian failure"); + } + return {Eigen::MatrixXd::Identity(2, 2), Eigen::MatrixXd::Zero(2, 1)}; + } +}; + +class ThrowingPrecomputeSolver : public CDDPSolverBase { +public: + void initialize(CDDP &context) override {} + + void runPrecomputeDynamicsDerivatives(CDDP &context, int min_horizon_for_parallel) { + precomputeDynamicsDerivatives(context, min_horizon_for_parallel); + } + + std::string getSolverName() const override { + return "ThrowingPrecomputeSolver"; + } + +protected: + bool backwardPass(CDDP &context) override { + return true; + } + + ForwardPassResult forwardPass(CDDP &context, double alpha) override { + ForwardPassResult result; + result.alpha_pr = alpha; + result.cost = 0.0; + result.merit_function = 0.0; + result.success = true; + return result; + } + + bool checkConvergence(CDDP &context, double dJ, double dL, int iter, + std::string &reason) override { + return false; + } + + void printIteration(int iter, const CDDP &context) const override {} +}; + // Factory functions for the mock solvers std::unique_ptr createMockExternalSolver() { return std::make_unique(); @@ -329,6 +384,31 @@ TEST_F(CDDPCoreTest, ParallelForwardPassKeepsSuccessfulAlphaWhenAnotherThrows) { EXPECT_DOUBLE_EQ(result.merit_function, 0.5); } +TEST_F(CDDPCoreTest, ParallelPrecomputeDynamicsDerivativesPropagatesExceptions) { + cddp::CDDPOptions parallel_options = options; + parallel_options.enable_parallel = true; + + Eigen::VectorXd local_initial_state = Eigen::VectorXd::Zero(2); + Eigen::VectorXd local_goal_state = Eigen::VectorXd::Zero(2); + + cddp::CDDP cddp_solver( + local_initial_state, local_goal_state, 4, timestep, + std::make_unique(timestep), + std::make_unique( + Eigen::MatrixXd::Identity(2, 2), Eigen::MatrixXd::Identity(1, 1), + Eigen::MatrixXd::Identity(2, 2), local_goal_state, + std::vector(), timestep), + parallel_options); + + cddp_solver.X_.assign(5, Eigen::VectorXd::Zero(2)); + cddp_solver.U_.assign(4, Eigen::VectorXd::Zero(1)); + + cddp::ThrowingPrecomputeSolver solver; + EXPECT_THROW( + solver.runPrecomputeDynamicsDerivatives(cddp_solver, 1), + std::runtime_error); +} + // Test solver precedence (external over built-in) TEST_F(CDDPCoreTest, SolverPrecedence) { // Register a solver with the same name as a built-in solver diff --git a/tests/cddp_core/test_clddp_solver.cpp b/tests/cddp_core/test_clddp_solver.cpp index 36a872e8..d85ffe91 100644 --- a/tests/cddp_core/test_clddp_solver.cpp +++ b/tests/cddp_core/test_clddp_solver.cpp @@ -16,9 +16,6 @@ #include #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include #include #include @@ -829,4 +826,3 @@ TEST(CLDDPTest, SolveQuadrotor) << "Warm start should also converge"; EXPECT_LE(warm_iterations, iterations_completed + 20) << "Warm start should not take significantly more iterations"; } - diff --git a/tests/cddp_core/test_constraint.cpp b/tests/cddp_core/test_constraint.cpp index 1f418629..dc8e75fa 100644 --- a/tests/cddp_core/test_constraint.cpp +++ b/tests/cddp_core/test_constraint.cpp @@ -17,9 +17,6 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" #include "cddp-cpp/cddp_core/constraint.hpp" -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include TEST(ControlConstraintTest, Evaluate) { @@ -235,90 +232,6 @@ TEST(LinearConstraintTest, Evaluate) { ASSERT_NEAR(constraint_value(0), 0.0, 1e-6); } -// New test case for LinearConstraint visualization -#ifdef CDDP_HAS_MATPLOT -TEST(LinearConstraintTest, Visualization) { - namespace plt = matplot; - - // 1. Define Linear Constraints (Ax <= b) - // Constraint 1: x + y <= 1 - // Constraint 2: -x + y <= 1 - Eigen::MatrixXd A(2, 2); - A << 1.0, 1.0, - -1.0, 1.0; - Eigen::VectorXd b(2); - b << 1.0, 1.0; - cddp::LinearConstraint constraint(A, b); - - // 2. Generate Grid of Points - double range = 2.0; - size_t num_points = 50; - std::vector x_vals = plt::linspace(-range, range, num_points); - std::vector y_vals = plt::linspace(-range, range, num_points); - auto [X, Y] = plt::meshgrid(x_vals, y_vals); - - // 3. Evaluate Constraint and Categorize Points - std::vector x_feasible, y_feasible; - std::vector x_infeasible, y_infeasible; - Eigen::VectorXd control(1); control << 0.0; // Dummy control - - for (size_t i = 0; i < X.size(); ++i) { - for (size_t j = 0; j < X[0].size(); ++j) { - Eigen::VectorXd state(2); - state << X[i][j], Y[i][j]; - Eigen::VectorXd constraint_value = constraint.evaluate(state, control); - // Check if ALL constraints are satisfied (Ax - b <= 0) - if (((A * state - b).array() <= 1e-6).all()) { // Use tolerance - x_feasible.push_back(state(0)); - y_feasible.push_back(state(1)); - } else { - x_infeasible.push_back(state(0)); - y_infeasible.push_back(state(1)); - } - } - } - - // // 4. Plotting (Commented out for CI/headless environments) - // auto fig = plt::figure(); - // plt::hold(true); - - // // Plot feasible and infeasible points - // if (!x_feasible.empty()) { - // plt::plot(x_feasible, y_feasible, "g.")->marker_size(5).display_name("Feasible (Ax <= b)"); - // } - // if (!x_infeasible.empty()) { - // plt::plot(x_infeasible, y_infeasible, "r.")->marker_size(5).display_name("Infeasible (Ax > b)"); - // } - - // // Plot constraint boundaries (Ax = b) - // // Line 1: x + y = 1 => y = 1 - x - // std::vector x_line1 = {-range, range}; - // std::vector y_line1 = {1 - x_line1[0], 1 - x_line1[1]}; - // plt::plot(x_line1, y_line1, "k-")->line_width(2).display_name("x + y = 1"); - - // // Line 2: -x + y = 1 => y = 1 + x - // std::vector x_line2 = {-range, range}; - // std::vector y_line2 = {1 + x_line2[0], 1 + x_line2[1]}; - // plt::plot(x_line2, y_line2, "b-")->line_width(2).display_name("-x + y = 1"); - - - // plt::hold(false); - // plt::xlabel("X"); - // plt::ylabel("Y"); - // plt::title("Linear Constraint Feasible Space (Ax <= b)"); - // plt::legend(); - // plt::grid(true); - // plt::axis("equal"); - // // plt::xlim({-range, range}); // Optional: Set limits - // // plt::ylim({-range, range}); - - // // Save the plot to a file - // std::string filename = "linear_constraint_visualization.png"; - // plt::save(filename); - // std::cout << "Saved linear constraint visualization to " << filename << std::endl; -} -#endif // CDDP_HAS_MATPLOT - // New test suite for SecondOrderConeConstraint TEST(SecondOrderConeConstraintTest, Evaluate) { Eigen::Vector3d origin(0.0, 0.0, 0.0); @@ -397,143 +310,6 @@ TEST(SecondOrderConeConstraintTest, Gradients) { ASSERT_TRUE(control_jacobian.isApprox(Eigen::MatrixXd::Zero(1, control.size()))); } -// New test case for visualization -#ifdef CDDP_HAS_MATPLOT -TEST(SecondOrderConeConstraintTest, Visualization) { - namespace plt = matplot; - - // 1. Define Cone Parameters - Eigen::Vector3d origin(0.0, 0.0, 0.0); - Eigen::Vector3d axis(0.0, 1.0, 0.0); // Changed to Y-axis - axis.normalize(); // Ensure axis is normalized - double fov = M_PI / 4.0; // 45 degrees half-angle - double tan_fov = std::tan(fov); - double epsilon = 1e-8; // Small regularization term - cddp::SecondOrderConeConstraint constraint(origin, axis, fov, epsilon); - - - // 2. Generate Cone Surface Points - std::vector h_vals = plt::linspace(0, 2, 20); // Height along opening axis (non-negative for this definition) - std::vector theta_vals = plt::linspace(0, 2 * M_PI, 40); // Angle around axis - - auto [H, THETA] = plt::meshgrid(h_vals, theta_vals); - - // Find orthogonal vectors u, v to the axis - Eigen::Vector3d temp_vec = (std::abs(axis.x()) > 1e-6 || std::abs(axis.z()) > 1e-6) ? Eigen::Vector3d(0,0,1) : Eigen::Vector3d(1,0,0); - Eigen::Vector3d u = axis.cross(temp_vec).normalized(); - Eigen::Vector3d v = axis.cross(u).normalized(); - - // Calculate X, Y, Z coordinates for the cone surface - std::vector> X(H.size(), std::vector(H[0].size())); - std::vector> Y(H.size(), std::vector(H[0].size())); - std::vector> Z(H.size(), std::vector(H[0].size())); - - for (size_t i = 0; i < H.size(); ++i) { - for (size_t j = 0; j < H[0].size(); ++j) { - double h = H[i][j]; - double theta = THETA[i][j]; - // Radius depends on the projection onto the axis, which is h - double radius = std::abs(h) * tan_fov; - Eigen::Vector3d point_on_circle = radius * (std::cos(theta) * u + std::sin(theta) * v); - Eigen::Vector3d point = origin + axis * h + point_on_circle; - X[i][j] = point.x(); - Y[i][j] = point.y(); - Z[i][j] = point.z(); - } - } - - // 3. Generate Sample Points (Inside/Outside) - Eigen::VectorXd control(1); control << 0.0; // Dummy control - - std::vector x_inside, y_inside, z_inside; - std::vector x_outside, y_outside, z_outside; - std::vector x_boundary, y_boundary, z_boundary; // Added for boundary points - - // Points to test (as Eigen::VectorXd for the constraint function) - // Cone opens along positive Y-axis - Eigen::VectorXd p_in_axis(3); p_in_axis << 0.0, 1.0, 0.0; // Inside: Along positive axis - Eigen::VectorXd p_in_near(3); p_in_near << 0.1, 0.5, 0.1; // Inside: Near origin - Eigen::VectorXd p_out_neg(3); p_out_neg << 0.0, -1.0, 0.0; // Outside: Along negative axis - Eigen::VectorXd p_out_far(3); p_out_far << 1.0, 1.0, 1.0; // Outside: Large radius - Eigen::VectorXd p_out_perp(3); p_out_perp << 1.0, 0.0, 0.0; // Outside: Perpendicular to axis at origin - - Eigen::VectorXd p_boundary1(3); // Boundary point at y=1 - double radius_at_h1 = std::abs(1.0) * tan_fov; - p_boundary1 << radius_at_h1, 1.0, 0.0; - - Eigen::VectorXd p_boundary2(3); // Boundary point at y=2 - double radius_at_h2 = std::abs(2.0) * tan_fov; - p_boundary2 << 0.0, 2.0, -radius_at_h2; // On Z-axis part of circle - - - // Check and categorize points - auto check_and_add = [&](const Eigen::VectorXd& p, const std::string& name) { - double val = constraint.evaluate(p, control)(0); - if (val < -1e-7) { // Allow small tolerance for strictly inside - x_inside.push_back(p(0)); y_inside.push_back(p(1)); z_inside.push_back(p(2)); - std::cout << name << " is inside (value: " << val << ")" << std::endl; - } else if (val > 1e-7) { // Allow small tolerance for strictly outside - x_outside.push_back(p(0)); y_outside.push_back(p(1)); z_outside.push_back(p(2)); - std::cout << name << " is outside (value: " << val << ")" << std::endl; - } else { // Consider it on the boundary - x_boundary.push_back(p(0)); y_boundary.push_back(p(1)); z_boundary.push_back(p(2)); // Store boundary points - std::cout << name << " is on boundary (value: " << val << ")" << std::endl; - // Removed plotting as inside - } - }; - - check_and_add(p_in_axis, "p_in_axis"); - check_and_add(p_in_near, "p_in_near"); - check_and_add(p_out_neg, "p_out_neg"); - check_and_add(p_out_far, "p_out_far"); - check_and_add(p_out_perp, "p_out_perp"); - check_and_add(p_boundary1, "p_boundary1"); - check_and_add(p_boundary2, "p_boundary2"); - - - // // // 4. Create Plot (Commented out for testing) - // auto fig = plt::figure(); - // plt::surf(X, Y, Z)->face_alpha(0.5).edge_color("none"); - // plt::hold(true); - - // // Plot feasible and infeasible points - // if (!x_inside.empty()) { - // plt::plot3(x_inside, y_inside, z_inside, "go")->marker_size(10).display_name("Inside"); // Changed display name - // } - // if (!x_outside.empty()) { - // plt::plot3(x_outside, y_outside, z_outside, "rx")->marker_size(10).display_name("Outside"); - // } - // if (!x_boundary.empty()) { // Added plotting for boundary points - // plt::plot3(x_boundary, y_boundary, z_boundary, "bs")->marker_size(10).display_name("Boundary"); - // } - - // // Plot origin and axis - // plt::plot3(std::vector{origin.x()}, std::vector{origin.y()}, std::vector{origin.z()}, "k*")->marker_size(12).display_name("Origin"); - // Eigen::Vector3d axis_start = origin - axis * 2.5; // Show axis extending both ways - // Eigen::Vector3d axis_end = origin + axis * 2.5; - // plt::plot3(std::vector{axis_start.x(), axis_end.x()}, - // std::vector{axis_start.y(), axis_end.y()}, - // std::vector{axis_start.z(), axis_end.z()}, "b-.")->line_width(2).display_name("Axis"); - - - // plt::hold(false); - // plt::xlabel("X"); - // plt::ylabel("Y"); - // plt::zlabel("Z"); - // plt::title("Second Order Cone Constraint Visualization"); - // plt::legend(); - // plt::grid(true); - // plt::axis("equal"); - // // plt::show(); - // // plt::view(45, 30); // Adjust view angle if needed - - // // Save the plot to a file - // std::string filename = "cone_visualization.png"; - // plt::save(filename); - // std::cout << "Saved cone visualization to " << filename << std::endl; -} -#endif // CDDP_HAS_MATPLOT - TEST(BallConstraintTest, Hessians) { // Create a ball constraint with a radius of 2.0 and scale_factor 1.0 cddp::BallConstraint constraint(2.0, Eigen::Vector2d(0.0, 0.0), 1.0); @@ -626,4 +402,4 @@ TEST(BallConstraintTest, Hessians) { // ASSERT_TRUE(Huu_list[0].isApprox(Eigen::MatrixXd::Zero(control.size(), control.size()))); // ASSERT_EQ(Hux_list.size(), 1); // ASSERT_TRUE(Hux_list[0].isApprox(Eigen::MatrixXd::Zero(control.size(), state.size()))); -// } \ No newline at end of file +// } diff --git a/tests/cddp_core/test_ipddp_solver.cpp b/tests/cddp_core/test_ipddp_solver.cpp index d7514f99..4e1d5019 100644 --- a/tests/cddp_core/test_ipddp_solver.cpp +++ b/tests/cddp_core/test_ipddp_solver.cpp @@ -16,9 +16,6 @@ #include #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include #include #include @@ -824,4 +821,3 @@ TEST(IPDDPTest, SolveQuadrotor) << "Warm start should also converge"; EXPECT_LE(warm_iterations, iterations_completed + 20) << "Warm start should not take significantly more iterations"; } - diff --git a/tests/cddp_core/test_logddp_solver.cpp b/tests/cddp_core/test_logddp_solver.cpp index b6abb5a8..a028d342 100644 --- a/tests/cddp_core/test_logddp_solver.cpp +++ b/tests/cddp_core/test_logddp_solver.cpp @@ -16,9 +16,6 @@ #include #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include #include #include @@ -834,4 +831,3 @@ TEST(LogDDPTest, SolveQuadrotor) << "Warm start should also converge"; EXPECT_LE(warm_iterations, iterations_completed + 20) << "Warm start should not take significantly more iterations"; } - diff --git a/tests/cddp_core/test_msipddp_core.cpp b/tests/cddp_core/test_msipddp_core.cpp index fdb3f9b3..45c94bd5 100644 --- a/tests/cddp_core/test_msipddp_core.cpp +++ b/tests/cddp_core/test_msipddp_core.cpp @@ -24,13 +24,6 @@ #include "gtest/gtest.h" #include "cddp.hpp" -#ifdef CDDP_HAS_MATPLOT -#include "matplot/matplot.h" -#endif - -#ifdef CDDP_HAS_MATPLOT -using namespace matplot; -#endif // namespace fs = std::filesystem; TEST(MSIPDDPTest, Solve) { diff --git a/tests/cddp_core/test_msipddp_solver.cpp b/tests/cddp_core/test_msipddp_solver.cpp index d26b6fa7..dbee2157 100644 --- a/tests/cddp_core/test_msipddp_solver.cpp +++ b/tests/cddp_core/test_msipddp_solver.cpp @@ -16,9 +16,6 @@ #include #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include #include #include @@ -824,4 +821,3 @@ TEST(MSIPDDPTest, SolveQuadrotor) << "Warm start should also converge"; EXPECT_LE(warm_iterations, iterations_completed + 20) << "Warm start should not take significantly more iterations"; } - diff --git a/tests/dynamics_model/test_attitude_dynamics.cpp b/tests/dynamics_model/test_attitude_dynamics.cpp index fb823e93..e3039787 100644 --- a/tests/dynamics_model/test_attitude_dynamics.cpp +++ b/tests/dynamics_model/test_attitude_dynamics.cpp @@ -5,9 +5,6 @@ #include "cddp_core/helper.hpp" // For attitude conversions #include #include -#ifdef CDDP_HAS_MATPLOT -#include // For plotting -#endif namespace cddp { @@ -213,7 +210,7 @@ namespace cddp } } - // --- Trajectory Comparison and Visualization --- + // --- Trajectory Comparison --- TEST_F(AttitudeDynamicsTest, TrajectoryComparison) { int num_steps = 500; @@ -268,204 +265,19 @@ namespace cddp omega_traj_quat_sim[k + 1] = current_state_quat.tail<3>(); } - // Plotting using matplotplusplus -#ifdef CDDP_HAS_MATPLOT - using namespace matplot; -#endif - - // Extract angle components for plotting - std::vector yaw_mrp, pitch_mrp, roll_mrp; - std::vector yaw_quat_euler, pitch_quat_euler, roll_quat_euler; - std::vector yaw_euler, pitch_euler, roll_euler; - std::vector omx_mrp, omy_mrp, omz_mrp; - std::vector omx_quat_sim, omy_quat_sim, omz_quat_sim; - std::vector omx_euler, omy_euler, omz_euler; - std::vector qw_mrp, qx_mrp, qy_mrp, qz_mrp; - std::vector qw_quat, qx_quat, qy_quat, qz_quat; - - for (const auto &angles : euler_traj_mrp) - { - yaw_mrp.push_back(angles(0)); - pitch_mrp.push_back(angles(1)); - roll_mrp.push_back(angles(2)); - } - for (const auto &angles : euler_traj_euler) - { - yaw_euler.push_back(angles(0)); - pitch_euler.push_back(angles(1)); - roll_euler.push_back(angles(2)); - } - for (const auto &angles : quat_traj_quat) - { - Eigen::Vector3d euler = cddp::helper::quatToEulerZYX(angles); - yaw_quat_euler.push_back(euler(0)); - pitch_quat_euler.push_back(euler(1)); - roll_quat_euler.push_back(euler(2)); - } - for (const auto &omega : omega_traj_mrp) - { - omx_mrp.push_back(omega(0)); - omy_mrp.push_back(omega(1)); - omz_mrp.push_back(omega(2)); - } - for (const auto &omega : omega_traj_euler) - { - omx_euler.push_back(omega(0)); - omy_euler.push_back(omega(1)); - omz_euler.push_back(omega(2)); - } - for (const auto &omega : omega_traj_quat_sim) - { - omx_quat_sim.push_back(omega(0)); - omy_quat_sim.push_back(omega(1)); - omz_quat_sim.push_back(omega(2)); - } - - // Extraction loops for quaternion components - for (const auto &quat : quat_traj_mrp) - { - qw_mrp.push_back(quat(0)); - qx_mrp.push_back(quat(1)); - qy_mrp.push_back(quat(2)); - qz_mrp.push_back(quat(3)); - } - for (const auto &quat : quat_traj_quat) - { - qw_quat.push_back(quat(0)); - qx_quat.push_back(quat(1)); - qy_quat.push_back(quat(2)); - qz_quat.push_back(quat(3)); - } - - // figure_handle fig = figure(true); // Create a new figure window - // fig->position({50, 50, 1600, 900}); // Adjusted size for more plots - - // int current_subplot = 0; - - // // Plot Euler Angles - // subplot(2, 3, current_subplot++); - // plot(t_eval, yaw_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, yaw_quat_euler, "g-")->line_width(2); - // plot(t_eval, yaw_euler, "b:")->line_width(2); - // hold(off); - // title("Yaw (psi) from Euler"); - // xlabel("Time (s)"); - // ylabel("Angle (rad)"); - // legend({"MRP->E", "Quat->E", "Euler"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, pitch_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, pitch_quat_euler, "g-")->line_width(2); - // plot(t_eval, pitch_euler, "b:")->line_width(2); - // hold(off); - // title("Pitch (theta) from Euler"); - // xlabel("Time (s)"); - // ylabel("Angle (rad)"); - // legend({"MRP->E", "Quat->E", "Euler"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, roll_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, roll_quat_euler, "g-")->line_width(2); - // plot(t_eval, roll_euler, "b:")->line_width(2); - // hold(off); - // title("Roll (phi) from Euler"); - // xlabel("Time (s)"); - // ylabel("Angle (rad)"); - // legend({"MRP->E", "Quat->E", "Euler"}); - // grid(on); - - // // Plot Angular Velocities - // subplot(2, 3, current_subplot++); - // plot(t_eval, omx_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, omx_quat_sim, "g-")->line_width(2); - // plot(t_eval, omx_euler, "b:")->line_width(2); - // hold(off); - // title("Omega X"); - // xlabel("Time (s)"); - // ylabel("Rate (rad/s)"); - // legend({"MRP", "Quaternion", "Euler"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, omy_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, omy_quat_sim, "g-")->line_width(2); - // plot(t_eval, omy_euler, "b:")->line_width(2); - // hold(off); - // title("Omega Y"); - // xlabel("Time (s)"); - // ylabel("Rate (rad/s)"); - // legend({"MRP", "Quaternion", "Euler"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, omz_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, omz_quat_sim, "g-")->line_width(2); - // plot(t_eval, omz_euler, "b:")->line_width(2); - // hold(off); - // title("Omega Z"); - // xlabel("Time (s)"); - // ylabel("Rate (rad/s)"); - // legend({"MRP", "Quaternion", "Euler"}); - // grid(on); - // // show(); - - // figure_handle fig_quat = figure(true); - // fig_quat->position({50, 50, 1600, 900}); - // current_subplot = 0; - - // subplot(2, 3, current_subplot++); - // plot(t_eval, qw_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, qw_quat, "g-")->line_width(2); - // hold(off); - // title("Quaternion w"); - // xlabel("Time (s)"); - // ylabel("Quaternion"); - // legend({"MRP", "Quaternion"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, qx_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, qx_quat, "g-")->line_width(2); - // hold(off); - // title("Quaternion x"); - // xlabel("Time (s)"); - // ylabel("Quaternion"); - // legend({"MRP", "Quaternion"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, qy_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, qy_quat, "g-")->line_width(2); - // hold(off); - // title("Quaternion y"); - // xlabel("Time (s)"); - // ylabel("Quaternion"); - // legend({"MRP", "Quaternion"}); - // grid(on); - - // subplot(2, 3, current_subplot++); - // plot(t_eval, qz_mrp, "r--")->line_width(2); - // hold(on); - // plot(t_eval, qz_quat, "g-")->line_width(2); - // hold(off); - // title("Quaternion z"); - // xlabel("Time (s)"); - // ylabel("Quaternion"); - // legend({"MRP", "Quaternion"}); - // grid(on); - - // show(); // Display plot window + ASSERT_EQ(t_eval.size(), static_cast(num_steps + 1)); + ASSERT_EQ(euler_traj_mrp.size(), static_cast(num_steps + 1)); + ASSERT_EQ(quat_traj_quat.size(), static_cast(num_steps + 1)); + ASSERT_EQ(quat_traj_mrp.size(), static_cast(num_steps + 1)); + EXPECT_TRUE(current_state_mrp.allFinite()); + EXPECT_TRUE(current_state_quat.allFinite()); + EXPECT_TRUE(current_state_euler.allFinite()); + EXPECT_TRUE(euler_traj_mrp.back().allFinite()); + EXPECT_TRUE(euler_traj_euler.back().allFinite()); + EXPECT_TRUE(quat_traj_quat.back().allFinite()); + EXPECT_TRUE(quat_traj_mrp.back().allFinite()); + EXPECT_TRUE(omega_traj_mrp.back().isApprox(omega_traj_quat_sim.back(), 1e-6)); + EXPECT_TRUE(omega_traj_mrp.back().isApprox(omega_traj_euler.back(), 1e-6)); } } // namespace tests diff --git a/tests/dynamics_model/test_mrp_attitude.cpp b/tests/dynamics_model/test_mrp_attitude.cpp index a4a792de..997b2bac 100644 --- a/tests/dynamics_model/test_mrp_attitude.cpp +++ b/tests/dynamics_model/test_mrp_attitude.cpp @@ -24,14 +24,7 @@ #include "cddp-cpp/dynamics_model/mrp_attitude.hpp" #include "cddp-cpp/cddp_core/helper.hpp" -#ifdef CDDP_HAS_MATPLOT -#include "matplot/matplot.h" -#endif - using namespace cddp; -#ifdef CDDP_HAS_MATPLOT -using namespace matplot; -#endif // Helper function for skew-symmetric matrix (double) Eigen::Matrix3d skew_double(const Eigen::Vector3d& v) { @@ -258,7 +251,6 @@ TEST_F(MrpAttitudeTest, SimulationAndPlotting) { // title(ax1, "MRP Components vs Time"); // xlabel(ax1, "Time [s]"); // ylabel(ax1, "MRP Value"); - // matplot::legend(ax1); // grid(ax1, on); // hold(ax1, off); @@ -271,7 +263,6 @@ TEST_F(MrpAttitudeTest, SimulationAndPlotting) { // title(ax2, "Angular Velocity vs Time"); // xlabel(ax2, "Time [s]"); // ylabel(ax2, "Angular Velocity [rad/s]"); - // matplot::legend(ax2); // grid(ax2, on); // hold(ax2, off); @@ -293,4 +284,4 @@ int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); ::testing::InitGoogleMock(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/dynamics_model/test_spacecraft_linear.cpp b/tests/dynamics_model/test_spacecraft_linear.cpp index fca35fe5..47a08a47 100644 --- a/tests/dynamics_model/test_spacecraft_linear.cpp +++ b/tests/dynamics_model/test_spacecraft_linear.cpp @@ -25,9 +25,6 @@ #include "gtest/gtest.h" #include "dynamics_model/spacecraft_linear.hpp" -#ifdef CDDP_HAS_MATPLOT -#include "matplot/matplot.h" -#endif using namespace cddp; @@ -143,73 +140,9 @@ TEST(HCWTest, RelativeTrajectory) { state = hcw.getDiscreteDynamics(state, control, 0.0); } - // // Plot the trajectory - // namespace plt = matplot; - // std::vector t_states_plot(num_steps + 1); - // for(int i = 0; i <= num_steps; ++i) t_states_plot[i] = i * timestep; - - - // // X-Y plane trajectory - // plt::figure(); - // plt::plot(y_pos_plot, x_pos_plot)->line_width(2).display_name("Trajectory"); - // plt::hold(true); - // if (!x_pos_plot.empty() && !y_pos_plot.empty()){ - // plt::scatter(std::vector{y_pos_plot.front()}, std::vector{x_pos_plot.front()}) - // ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - // plt::scatter(std::vector{y_pos_plot.back()}, std::vector{x_pos_plot.back()}) - // ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - // } - // plt::hold(false); - // plt::xlabel("y (m) [In-track]"); - // plt::ylabel("x (m) [Radial]"); - // plt::legend(); - // plt::title("HCW X-Y Plane Trajectory"); - // plt::axis(plt::equal); - // plt::gca()->x_axis().reverse(true); - - // // 3D Trajectory - // plt::figure(); - // plt::plot3(x_pos_plot, y_pos_plot, z_pos_plot, "-o")->line_width(2).marker_size(4).display_name("Trajectory"); - // plt::hold(true); - // if (!x_pos_plot.empty()){ - // plt::scatter3(std::vector{x_pos_plot.front()}, std::vector{y_pos_plot.front()}, std::vector{z_pos_plot.front()}) - // ->marker_color("g").marker_style("o").marker_size(10).display_name("Start"); - // plt::scatter3(std::vector{x_pos_plot.back()}, std::vector{y_pos_plot.back()}, std::vector{z_pos_plot.back()}) - // ->marker_color("r").marker_style("x").marker_size(10).display_name("End"); - // } - // plt::hold(false); - // plt::xlabel("x (m) [Radial]"); - // plt::ylabel("y (m) [In-track]"); - // plt::zlabel("z (m) [Cross-track]"); - // plt::legend(); - // plt::title("3D HCW Trajectory"); - // plt::axis(plt::equal); - - // plt::show(); // Show all plots -} - -// Helper function to create spacecraft marker coordinates -std::vector> createSpacecraftMarker( - const Eigen::Vector3d& position, - double size = 1.0) { - - std::vector> marker(3, std::vector()); - - // Simple cube-like spacecraft shape - std::vector dx = {-1, 1, 1, -1, -1, -1, 1, 1, -1}; - std::vector dy = {-1, -1, 1, 1, -1, -1, -1, 1, 1}; - std::vector dz = {-1, -1, -1, -1, -1, 1, 1, 1, 1}; - - for (size_t i = 0; i < dx.size(); ++i) { - marker[0].push_back(position.x() + size * dx[i]); - marker[1].push_back(position.y() + size * dy[i]); - marker[2].push_back(position.z() + size * dz[i]); - } - - return marker; } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/dynamics_model/test_spacecraft_linear_fuel.cpp b/tests/dynamics_model/test_spacecraft_linear_fuel.cpp index 1494f719..2bdeef5e 100644 --- a/tests/dynamics_model/test_spacecraft_linear_fuel.cpp +++ b/tests/dynamics_model/test_spacecraft_linear_fuel.cpp @@ -27,10 +27,6 @@ #include "dynamics_model/spacecraft_linear_fuel.hpp" #include "cddp.hpp" using namespace cddp; -#ifdef CDDP_HAS_MATPLOT -using namespace matplot; -namespace plt = matplot; -#endif TEST(SpacecraftLinearFuelTest, DiscreteDynamics) { // Create an SpacecraftLinearFuel instance @@ -127,46 +123,9 @@ TEST(SpacecraftLinearFuelTest, RelativeTrajectory) { states[i + 1] = model.getDiscreteDynamics(states[i], control, 0.0); } - // Store trajectory points - std::vector x_data, y_data, z_data, vx_data, vy_data, vz_data, mass_data, control_x_data, control_y_data, control_z_data; - for (int i = 0; i < num_steps + 1; ++i) { - x_data.push_back(states[i](0)); - y_data.push_back(states[i](1)); - z_data.push_back(states[i](2)); - vx_data.push_back(states[i](3)); - vy_data.push_back(states[i](4)); - vz_data.push_back(states[i](5)); - mass_data.push_back(states[i](6)); - } - - // Plot 3d trajectory - // plt::figure(); - // plt::plot3(x_data, y_data, z_data, "r-"); - // plt::show(); -} - -// Helper function to create spacecraft marker coordinates -std::vector> createSpacecraftMarker( - const Eigen::Vector3d& position, - double size = 1.0) { - - std::vector> marker(3, std::vector()); - - // Simple cube-like spacecraft shape - std::vector dx = {-1, 1, 1, -1, -1, -1, 1, 1, -1}; - std::vector dy = {-1, -1, 1, 1, -1, -1, -1, 1, 1}; - std::vector dz = {-1, -1, -1, -1, -1, 1, 1, 1, 1}; - - for (size_t i = 0; i < dx.size(); ++i) { - marker[0].push_back(position.x() + size * dx[i]); - marker[1].push_back(position.y() + size * dy[i]); - marker[2].push_back(position.z() + size * dz[i]); - } - - return marker; } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/dynamics_model/test_spacecraft_nonlinear.cpp b/tests/dynamics_model/test_spacecraft_nonlinear.cpp index a458ef5c..ea89b38f 100644 --- a/tests/dynamics_model/test_spacecraft_nonlinear.cpp +++ b/tests/dynamics_model/test_spacecraft_nonlinear.cpp @@ -21,10 +21,6 @@ #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif - #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -34,17 +30,11 @@ using namespace cddp; TEST(SpacecraftNonlinearTest, DiscreteDynamics) { // Create a spacecraft instance double timestep = 0.01; // 1s timestep - double mu = 1.0; // Earth's gravitational parameter (normalized) double mass = 1.0; // 1 kg spacecraft std::string integration_type = "rk4"; SpacecraftNonlinear spacecraft(timestep, integration_type, mass); - // Store states for plotting - std::vector time_data, px_data, py_data, pz_data; - std::vector vx_data, vy_data, vz_data; - std::vector r0_data, theta_data; - // Initial state: Eigen::VectorXd state = Eigen::VectorXd::Zero(10); state << -0.01127, @@ -64,20 +54,11 @@ TEST(SpacecraftNonlinearTest, DiscreteDynamics) { // Simulate for several orbits int num_steps = 3000; for (int i = 0; i < num_steps; ++i) { - // Store data for plotting - time_data.push_back(i * timestep); - px_data.push_back(state[0]); - py_data.push_back(state[1]); - pz_data.push_back(state[2]); - vx_data.push_back(state[3]); - vy_data.push_back(state[4]); - vz_data.push_back(state[5]); - r0_data.push_back(state[6]); - theta_data.push_back(state[7]); - // Compute the next state state = spacecraft.getDiscreteDynamics(state, control, 0.0); } + + EXPECT_TRUE(state.allFinite()); } TEST(SpacecraftNonlinearTest, AlfriendExample4_1_RelativeMotion) { @@ -169,92 +150,6 @@ TEST(SpacecraftNonlinearTest, AlfriendExample4_1_RelativeMotion) { FAIL() << "Simulation duration is less than one chief orbital period, cannot check periodicity."; } -#ifdef CDDP_HAS_MATPLOT - namespace plt = matplot; -#endif - std::vector time_plot_data; - std::vector px_plot_data, py_plot_data, pz_plot_data; - - for(int i=0; i<=num_steps; ++i){ - time_plot_data.push_back(i * timestep); - px_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PX)); - py_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PY)); - pz_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PZ)); - } - - // // Figure 4.2: Time histories of normalized relative position components - // plt::figure(); - // plt::subplot(3, 1, 0); - // plt::plot(time_plot_data, px_plot_data); - // plt::ylabel("x/a"); - // plt::title("Figure 4.2: Normalized Relative Position vs. Time"); - - // plt::subplot(3, 1, 1); - // plt::plot(time_plot_data, py_plot_data); - // plt::ylabel("y/a"); - - // plt::subplot(3, 1, 2); - // plt::plot(time_plot_data, pz_plot_data); - // plt::ylabel("z/a"); - // plt::xlabel("Orbital Periods (Chief)"); // The example image shows this, time is normalized by period - // // Adjust x-axis to show orbital periods - // std::vector time_in_periods; - // for(double t : time_plot_data) time_in_periods.push_back(t / chief_orbital_period); - // plt::figure(); // Create a new figure for clarity with corrected x-axis - // plt::subplot(3, 1, 0); - // plt::plot(time_in_periods, px_plot_data); - // plt::ylabel("x/a"); - // plt::ylim({-0.02, 0.02}); - // plt::title("Figure 4.2 Style: Normalized Relative Position vs. Chief Orbital Periods"); - - // plt::subplot(3, 1, 1); - // plt::plot(time_in_periods, py_plot_data); - // plt::ylabel("y/a"); - // plt::ylim({-0.1, 0.1}); - - // plt::subplot(3, 1, 2); - // plt::plot(time_in_periods, pz_plot_data); - // plt::ylabel("z/a"); - // plt::ylim({-0.2, 0.2}); - // plt::xlabel("Chief Orbital Periods"); - - - // // Figure 4.3: Relative motion in configuration space - // plt::figure(); - // // y/a vs x/a - // plt::subplot(2, 2, 0); // Corresponds to top-left in a 2x2 grid - // plt::plot(px_plot_data, py_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("y/a"); - // plt::axis(plt::equal); - // plt::title("Figure 4.3 Style: Configuration Space Projections"); - - // // z/a vs x/a - // plt::subplot(2, 2, 1); // Corresponds to top-right - // plt::plot(px_plot_data, pz_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("z/a"); - // plt::axis(plt::equal); - - // // z/a vs y/a (Note: Fig 4.3 shows this plot with y/a on horizontal axis) - // plt::subplot(2, 2, 2); // Corresponds to bottom-left - // plt::plot(py_plot_data, pz_plot_data); - // plt::xlabel("y/a"); - // plt::ylabel("z/a"); - // plt::axis(plt::equal); - - // // 3D plot: x/a, y/a, z/a - // plt::subplot(2, 2, 3); // Corresponds to bottom-right - // plt::plot3(px_plot_data, py_plot_data, pz_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("y/a"); - // plt::zlabel("z/a"); - // plt::axis(plt::equal); - // // For view to match Fig 4.3 (approximate) - // // matplot++ view control is plt::view(azimuth, elevation); - // plt::view(120, 30); // Adjust these values as needed - - // plt::show(); } TEST(SpacecraftNonlinearTest, RelativeMotion2) { @@ -328,125 +223,11 @@ TEST(SpacecraftNonlinearTest, RelativeMotion2) { state_history.push_back(current_state); } - // --- Assertions --- - - // int steps_per_period = static_cast(std::round(chief_orbital_period / timestep)); - // int start_step_for_assertion = num_steps - 2 * steps_per_period; - // if (start_step_for_assertion < 1) start_step_for_assertion = 1; - - // for (int i = start_step_for_assertion; i <= num_steps; ++i) { - // // Bounds from Fig 4.2 (x/a, y/a, z/a which are px, py, pz here) - // ASSERT_GE(state_history[i](SpacecraftNonlinear::STATE_PX), -0.019) << "px at step " << i << " is too low."; - // ASSERT_LE(state_history[i](SpacecraftNonlinear::STATE_PX), 0.019) << "px at step " << i << " is too high."; - // ASSERT_GE(state_history[i](SpacecraftNonlinear::STATE_PY), -0.101) << "py at step " << i << " is too low."; - // ASSERT_LE(state_history[i](SpacecraftNonlinear::STATE_PY), 0.101) << "py at step " << i << " is too high."; - // ASSERT_GE(state_history[i](SpacecraftNonlinear::STATE_PZ), -0.181) << "pz at step " << i << " is too low."; - // ASSERT_LE(state_history[i](SpacecraftNonlinear::STATE_PZ), 0.181) << "pz at step " << i << " is too high."; - // } - - // if (num_steps >= steps_per_period) { - // Eigen::VectorXd state_at_3_periods = state_history[num_steps - steps_per_period]; - // Eigen::VectorXd state_at_4_periods = state_history[num_steps]; - - // double tol = 5e-4; - - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_PX), state_at_3_periods(SpacecraftNonlinear::STATE_PX), tol); - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_PY), state_at_3_periods(SpacecraftNonlinear::STATE_PY), tol); - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_PZ), state_at_3_periods(SpacecraftNonlinear::STATE_PZ), tol); - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_VX), state_at_3_periods(SpacecraftNonlinear::STATE_VX), tol); - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_VY), state_at_3_periods(SpacecraftNonlinear::STATE_VY), tol); - // ASSERT_NEAR(state_at_4_periods(SpacecraftNonlinear::STATE_VZ), state_at_3_periods(SpacecraftNonlinear::STATE_VZ), tol); - // } else { - // FAIL() << "Simulation duration is less than one chief orbital period, cannot check periodicity."; - // } - - // namespace plt = matplot; - // std::vector time_plot_data; - // std::vector px_plot_data, py_plot_data, pz_plot_data; - - // for(int i=0; i<=num_steps; ++i){ - // time_plot_data.push_back(i * timestep); - // px_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PX) * ref_semimajor_axis); - // py_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PY) * ref_semimajor_axis); - // pz_plot_data.push_back(state_history[i](SpacecraftNonlinear::STATE_PZ) * ref_semimajor_axis); - // } - - // // Figure 4.2: Time histories of normalized relative position components - // plt::figure(); - // plt::subplot(3, 1, 0); - // plt::plot(time_plot_data, px_plot_data); - // plt::ylabel("x/a"); - // plt::title("Figure 4.2: Normalized Relative Position vs. Time"); - - // plt::subplot(3, 1, 1); - // plt::plot(time_plot_data, py_plot_data); - // plt::ylabel("y/a"); - - // plt::subplot(3, 1, 2); - // plt::plot(time_plot_data, pz_plot_data); - // plt::ylabel("z/a"); - // plt::xlabel("Orbital Periods (Chief)"); // The example image shows this, time is normalized by period - // // Adjust x-axis to show orbital periods - // std::vector time_in_periods; - // for(double t : time_plot_data) time_in_periods.push_back(t / chief_orbital_period); - // plt::figure(); // Create a new figure for clarity with corrected x-axis - // plt::subplot(3, 1, 0); - // plt::plot(time_in_periods, px_plot_data); - // plt::ylabel("x/a"); - // plt::ylim({-0.02, 0.02}); - // plt::title("Figure 4.2 Style: Normalized Relative Position vs. Chief Orbital Periods"); - - // plt::subplot(3, 1, 1); - // plt::plot(time_in_periods, py_plot_data); - // plt::ylabel("y/a"); - // plt::ylim({-0.1, 0.1}); - - // plt::subplot(3, 1, 2); - // plt::plot(time_in_periods, pz_plot_data); - // plt::ylabel("z/a"); - // plt::ylim({-0.2, 0.2}); - // plt::xlabel("Chief Orbital Periods"); - - - // // Figure 4.3: Relative motion in configuration space - // plt::figure(); - // // y/a vs x/a - // plt::subplot(2, 2, 0); // Corresponds to top-left in a 2x2 grid - // plt::plot(px_plot_data, py_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("y/a"); - // plt::axis(plt::equal); - // plt::title("Figure 4.3 Style: Configuration Space Projections"); - - // // z/a vs x/a - // plt::subplot(2, 2, 1); // Corresponds to top-right - // plt::plot(px_plot_data, pz_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("z/a"); - // plt::axis(plt::equal); - - // // z/a vs y/a (Note: Fig 4.3 shows this plot with y/a on horizontal axis) - // plt::subplot(2, 2, 2); // Corresponds to bottom-left - // plt::plot(py_plot_data, pz_plot_data); - // plt::xlabel("y/a"); - // plt::ylabel("z/a"); - // plt::axis(plt::equal); - - // // 3D plot: x/a, y/a, z/a - // plt::subplot(2, 2, 3); // Corresponds to bottom-right - // plt::plot3(px_plot_data, py_plot_data, pz_plot_data); - // plt::xlabel("x/a"); - // plt::ylabel("y/a"); - // plt::zlabel("z/a"); - // plt::axis(plt::equal); - // // For view to match Fig 4.3 (approximate) - // // matplot++ view control is plt::view(azimuth, elevation); - // plt::view(120, 30); // Adjust these values as needed - - // plt::show(); + ASSERT_EQ(state_history.size(), static_cast(num_steps + 1)); + EXPECT_TRUE(state_history.back().allFinite()); } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/dynamics_model/test_spacecraft_roe.cpp b/tests/dynamics_model/test_spacecraft_roe.cpp index 2139000b..e724250d 100644 --- a/tests/dynamics_model/test_spacecraft_roe.cpp +++ b/tests/dynamics_model/test_spacecraft_roe.cpp @@ -12,9 +12,6 @@ #include #include #include -#ifdef CDDP_HAS_MATPLOT -#include -#endif #include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/spacecraft_linear.hpp" From 098ece103a01125af26ba0090bbf4cf6ef3ef21a Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 04:04:30 -0400 Subject: [PATCH 07/14] Fix GTest installation during test builds Force INSTALL_GTEST off before fetching googletest so enabling CDDP_CPP_BUILD_TESTS does not install the bundled dependency alongside cddp. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a48da54f..47c7cb44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -109,6 +109,7 @@ FetchContent_MakeAvailable(autodiff) # Googletest if (CDDP_CPP_BUILD_TESTS) enable_testing() + set(INSTALL_GTEST OFF CACHE BOOL "Don't install GTest alongside cddp" FORCE) FetchContent_Declare( googletest GIT_REPOSITORY https://github.com/google/googletest.git From d3b220b040b827367697fe2ad6320cfa8878f234 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 14:10:54 -0400 Subject: [PATCH 08/14] Fix Python wrapper cleanup and forward pass diagnostics Release Python-owned wrapper references under the GIL so bound solver components are cleaned up safely. Also log partial forward-pass thread failures in verbose mode to make solver issues easier to diagnose. --- python/src/bind_solver.cpp | 32 ++++++++++++++++++++++++++++-- src/cddp_core/cddp_solver_base.cpp | 5 +++++ 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index cd367db2..08cd8614 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -42,9 +42,10 @@ void validateInitialTrajectory(cddp::CDDP &solver, try { state_dim = solver.getStateDim(); control_dim = solver.getControlDim(); - } catch (const std::exception &) { + } catch (const std::exception &e) { throw py::value_error( - "set_initial_trajectory requires a dynamical system to be set first."); + std::string("set_initial_trajectory failed while querying dimensions " + "(is a dynamical system set?): ") + e.what()); } const std::size_t expected_state_count = @@ -95,6 +96,15 @@ class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { wrapped->getIntegrationType()), owner_(std::move(owner)), wrapped_(wrapped) {} + ~PythonBackedDynamicalSystem() override { + try { + if (Py_IsInitialized()) { + py::gil_scoped_acquire gil; + owner_.release().dec_ref(); + } + } catch (...) {} + } + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const override { @@ -163,6 +173,15 @@ class PythonBackedObjective : public cddp::Objective { PythonBackedObjective(py::object owner, cddp::Objective *wrapped) : owner_(std::move(owner)), wrapped_(wrapped) {} + ~PythonBackedObjective() override { + try { + if (Py_IsInitialized()) { + py::gil_scoped_acquire gil; + owner_.release().dec_ref(); + } + } catch (...) {} + } + double evaluate(const std::vector &states, const std::vector &controls) const override { py::gil_scoped_acquire gil; @@ -265,6 +284,15 @@ class PythonBackedConstraint : public cddp::Constraint { : cddp::Constraint(wrapped->getName()), owner_(std::move(owner)), wrapped_(wrapped) {} + ~PythonBackedConstraint() override { + try { + if (Py_IsInitialized()) { + py::gil_scoped_acquire gil; + owner_.release().dec_ref(); + } + } catch (...) {} + } + int getDualDim() const override { py::gil_scoped_acquire gil; return wrapped_->getDualDim(); diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index 87126fbf..870a5e46 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -302,6 +302,11 @@ ForwardPassResult CDDPSolverBase::performForwardPass(CDDP &context) { std::cerr << getSolverName() << ": ALL forward pass threads failed. Last error: " << last_error << std::endl; + } else if (failed_count > 0 && options.verbose) { + std::cerr << getSolverName() << ": " << failed_count << " of " + << futures.size() + << " forward pass threads failed. Last error: " << last_error + << std::endl; } if (first_exception && !best_result.success) { std::rethrow_exception(first_exception); From d4bcf75762b19110df194cb8283ea32f63365477 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 16:04:25 -0400 Subject: [PATCH 09/14] Fix Python solver error message regression test Update the regression assertion to match the current actionable\nbinding message raised when set_initial_trajectory is called\nbefore a dynamical system is configured. --- python/tests/test_solver_errors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/tests/test_solver_errors.py b/python/tests/test_solver_errors.py index a7c2cc95..38a47289 100644 --- a/python/tests/test_solver_errors.py +++ b/python/tests/test_solver_errors.py @@ -61,7 +61,7 @@ def test_set_initial_trajectory_requires_dynamical_system(): X = [np.zeros(2) for _ in range(solver.horizon + 1)] U = [np.zeros(1) for _ in range(solver.horizon)] - with pytest.raises(ValueError, match="requires a dynamical system"): + with pytest.raises(ValueError, match="is a dynamical system set"): solver.set_initial_trajectory(X, U) From af1ccfd4cc91aa9ea6cc6d5735e5f9803324679b Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 17:02:56 -0400 Subject: [PATCH 10/14] Remove extra blank line in bind_solver.cpp --- python/src/bind_solver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 08cd8614..65fde2b3 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -298,7 +298,6 @@ class PythonBackedConstraint : public cddp::Constraint { return wrapped_->getDualDim(); } - Eigen::VectorXd evaluate(const Eigen::VectorXd &state, const Eigen::VectorXd &control, int index) const override { From 9b9b7e959e8a67e4c2f28887bf77036c6edf81e9 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 22:15:00 -0400 Subject: [PATCH 11/14] Fix constraint base destructor and import error test --- include/cddp-cpp/cddp_core/constraint.hpp | 2 ++ python/tests/test_solver_errors.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/cddp-cpp/cddp_core/constraint.hpp b/include/cddp-cpp/cddp_core/constraint.hpp index 6ce5064e..9fa708db 100644 --- a/include/cddp-cpp/cddp_core/constraint.hpp +++ b/include/cddp-cpp/cddp_core/constraint.hpp @@ -34,6 +34,8 @@ namespace cddp // Constructor Constraint(const std::string &name) : name_(name) {} + virtual ~Constraint() = default; + // Get the name of the constraint const std::string &getName() const { return name_; } diff --git a/python/tests/test_solver_errors.py b/python/tests/test_solver_errors.py index 38a47289..46871051 100644 --- a/python/tests/test_solver_errors.py +++ b/python/tests/test_solver_errors.py @@ -109,7 +109,7 @@ def test_import_error_message_is_actionable(tmp_path): shutil.copy(source_dir / "_version.py", package_dir / "_version.py") proc = subprocess.run( - [sys.executable, "-c", "import pycddp"], + [sys.executable, "-s", "-c", "import pycddp"], env={"PYTHONPATH": str(tmp_path)}, capture_output=True, text=True, From 2eb8399e49a4d996c00fb486770226b7683ba374 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 30 Mar 2026 22:54:02 -0400 Subject: [PATCH 12/14] Fix Python build dependencies and install paths Pin pybind11 to <3 to avoid incompatibility with older bindings. Install the Python extension and package files into the pycddp target directory directly. --- pyproject.toml | 2 +- python/CMakeLists.txt | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index e059eaa1..967ea699 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [build-system] -requires = ["scikit-build-core>=0.8", "pybind11>=2.12"] +requires = ["scikit-build-core>=0.8", "pybind11>=2.12,<3"] build-backend = "scikit_build_core.build" [project] diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d0242225..8a12a13d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -25,14 +25,10 @@ target_include_directories(_pycddp_core PRIVATE ${CMAKE_SOURCE_DIR}/include/cddp-cpp ) -set(PYCDDP_INSTALL_DIR - "${CMAKE_INSTALL_LIBDIR}/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages/pycddp" -) - -install(TARGETS _pycddp_core DESTINATION "${PYCDDP_INSTALL_DIR}") +install(TARGETS _pycddp_core DESTINATION pycddp) install( FILES ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/__init__.py ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/_version.py - DESTINATION "${PYCDDP_INSTALL_DIR}" + DESTINATION pycddp ) From eb4c208f5136544f077214947c9205c0ff622b68 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 31 Mar 2026 01:04:30 -0400 Subject: [PATCH 13/14] Add ALDDP solver implementation and tests Wire the augmented Lagrangian DDP solver into the core library, expose its options in the Python bindings, and register it as a built-in solver type. Add focused ALDDP coverage for constrained, infeasible-start, and square-root backward-pass scenarios. --- CMakeLists.txt | 1 + include/cddp-cpp/cddp.hpp | 1 + include/cddp-cpp/cddp_core/alddp_solver.hpp | 141 +++ include/cddp-cpp/cddp_core/cddp_core.hpp | 3 +- include/cddp-cpp/cddp_core/options.hpp | 38 + python/src/bind_options.cpp | 23 +- python/src/bind_solver.cpp | 4 +- src/cddp_core/alddp_solver.cpp | 1180 +++++++++++++++++++ src/cddp_core/cddp_core.cpp | 7 +- tests/CMakeLists.txt | 4 + tests/cddp_core/test_alddp_solver.cpp | 599 ++++++++++ 11 files changed, 1995 insertions(+), 6 deletions(-) create mode 100644 include/cddp-cpp/cddp_core/alddp_solver.hpp create mode 100644 src/cddp_core/alddp_solver.cpp create mode 100644 tests/cddp_core/test_alddp_solver.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 47c7cb44..8ea3060f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,6 +138,7 @@ set(cddp_core_srcs src/cddp_core/logddp_solver.cpp src/cddp_core/ipddp_solver.cpp src/cddp_core/msipddp_solver.cpp + src/cddp_core/alddp_solver.cpp ) set(dynamics_model_srcs diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index 27e6fccf..bd8d438a 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -32,6 +32,7 @@ #include "cddp_core/logddp_solver.hpp" #include "cddp_core/ipddp_solver.hpp" #include "cddp_core/msipddp_solver.hpp" +#include "cddp_core/alddp_solver.hpp" #include "cddp_core/helper.hpp" #include "cddp_core/boxqp.hpp" #include "cddp_core/qp_solver.hpp" diff --git a/include/cddp-cpp/cddp_core/alddp_solver.hpp b/include/cddp-cpp/cddp_core/alddp_solver.hpp new file mode 100644 index 00000000..7a1b84f1 --- /dev/null +++ b/include/cddp-cpp/cddp_core/alddp_solver.hpp @@ -0,0 +1,141 @@ +/* + 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_ALDDP_SOLVER_HPP +#define CDDP_ALDDP_SOLVER_HPP + +#include "cddp_core/boxqp.hpp" +#include "cddp_core/cddp_solver_base.hpp" +#include "cddp_core/constraint.hpp" +#include +#include +#include +#include + +namespace cddp { + +/** + * @brief Augmented Lagrangian DDP (ALDDP) solver implementation. + * + * Implements an ALTRO-like solver that uses augmented Lagrangian penalization + * to handle general nonlinear state and control constraints. Features: + * - AL outer loop with multiplier/penalty updates + * - BoxQP for control bounds (optional, like CLDDP) + * - Infeasible state trajectory initialization via slack controls + * - Square-root backward pass for numerical robustness + */ +class ALDDPSolver : public CDDPSolverBase { +public: + ALDDPSolver(); + + void initialize(CDDP &context) override; + std::string getSolverName() const override; + +protected: + // CDDPSolverBase hooks + void preIterationSetup(CDDP &context) override; + bool backwardPass(CDDP &context) override; + bool checkEarlyConvergence(CDDP &context, int iter, + std::string &reason) 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 handleBackwardPassRegularizationLimit( + 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: + // --- AL variables --- + // Per-constraint, per-timestep Lagrange multipliers (dim = getDualDim()) + std::map> lambda_; + // Per-constraint, per-timestep penalty weights + std::map> penalty_; + // Terminal constraint multipliers and penalties + std::map terminal_lambda_; + std::map terminal_penalty_; + + // Constraint evaluation cache (only written by evaluateConstraints, read by backward/forward pass) + std::map> G_; + std::map G_terminal_; + std::map> G_x_; + std::map> G_u_; + + // BoxQP for control bounds + BoxQPSolver boxqp_solver_; + + // AL outer loop state + int al_outer_iter_ = 0; + double inner_tolerance_ = 1e-2; + double max_constraint_violation_ = 0.0; + double prev_max_constraint_violation_ = 0.0; + double prev_outer_cost_ = 0.0; + bool inner_converged_ = false; + int inner_iter_count_ = 0; + + // Infeasible initialization + bool infeasible_start_ = false; + double current_slack_penalty_ = 1000.0; + std::vector S_; // Slack controls s_k (state_dim each) + std::vector k_s_; // Slack feedforward gains + std::vector K_s_; // Slack feedback gains + + // Square-root backward pass workspace + std::vector S_chol_; // Upper-triangular Cholesky factors √P_k + std::vector p_; // Cost-to-go gradient vectors + + // --- Private helpers --- + void initializeMultipliersAndPenalties(CDDP &context); + void initializeSlackControls(CDDP &context); + void evaluateConstraints(CDDP &context); + void updateMultipliers(CDDP &context); + void updatePenalties(CDDP &context); + double computeMaxConstraintViolation() const; + double computeMaxSlackNorm() const; + + // Backward pass variants + bool backwardPassStandard(CDDP &context); + bool backwardPassSqrt(CDDP &context); + + // AL cost derivative augmentation helpers + void augmentRunningCostDerivatives( + const CDDP &context, int t, + const Eigen::VectorXd &x, const Eigen::VectorXd &u, + Eigen::VectorXd &l_x, Eigen::VectorXd &l_u, + Eigen::MatrixXd &l_xx, Eigen::MatrixXd &l_uu, + Eigen::MatrixXd &l_ux) const; + + void augmentTerminalCostDerivatives( + const CDDP &context, const Eigen::VectorXd &x_N, + Eigen::VectorXd &V_x, Eigen::MatrixXd &V_xx) const; + + // Compute AL merit for a given trajectory (thread-safe, no member writes) + double computeALMerit( + const CDDP &context, + const std::vector &X, + const std::vector &U, + const std::vector *S_trial = nullptr) const; +}; + +} // namespace cddp + +#endif // CDDP_ALDDP_SOLVER_HPP diff --git a/include/cddp-cpp/cddp_core/cddp_core.hpp b/include/cddp-cpp/cddp_core/cddp_core.hpp index 4ae02106..7b86d0e5 100644 --- a/include/cddp-cpp/cddp_core/cddp_core.hpp +++ b/include/cddp-cpp/cddp_core/cddp_core.hpp @@ -45,7 +45,8 @@ enum class SolverType { CLDDP, ///< Control-Limited Differential Dynamic Programming LogDDP, ///< Log-Barrier Differential Dynamic Programming IPDDP, ///< Interior Point Differential Dynamic Programming - MSIPDDP ///< Multi-Shooting Interior Point Differential Dynamic Programming + MSIPDDP, ///< Multi-Shooting Interior Point Differential Dynamic Programming + ALDDP ///< Augmented Lagrangian Differential Dynamic Programming }; /** diff --git a/include/cddp-cpp/cddp_core/options.hpp b/include/cddp-cpp/cddp_core/options.hpp index cee6bb9c..656b2153 100644 --- a/include/cddp-cpp/cddp_core/options.hpp +++ b/include/cddp-cpp/cddp_core/options.hpp @@ -164,6 +164,42 @@ namespace cddp double terminal_constraint_tolerance = 1e-6; ///< Tolerance for terminal constraint satisfaction. }; + /** + * @brief Options for the ALDDP (Augmented Lagrangian DDP) solver. + * + * ALDDP uses an augmented Lagrangian method to handle general nonlinear + * state and control constraints. The solver alternates between an inner + * iLQR loop (which minimizes the augmented Lagrangian) and an outer loop + * that updates Lagrange multipliers and penalty weights. + */ + struct ALDDPAlgorithmOptions + { + // Multiplier/penalty initialization + double lambda_init = 0.0; ///< Initial Lagrange multiplier value. + double penalty_init = 1.0; ///< Initial penalty weight. + double penalty_max = 1e8; ///< Maximum penalty weight. + double penalty_update_factor = 10.0; ///< Factor to increase penalties. + + // Inner loop control + int max_inner_iterations = 50; ///< Max iLQR iterations per AL outer iteration. + double inner_tolerance_init = 1e-2; ///< Initial inner iLQR convergence tolerance. + double inner_tolerance_factor = 0.1; ///< Factor to tighten inner tolerance. + double inner_tolerance_min = 1e-6; ///< Minimum inner tolerance. + + // Outer loop control + int max_outer_iterations = 20; ///< Max AL outer iterations. + double constraint_tolerance = 1e-6; ///< Constraint satisfaction tolerance. + + // Control constraint handling + bool use_boxqp_for_controls = true; ///< Use BoxQP for control bounds (like CLDDP). + + // Infeasible initialization (ALTRO Eqs. 30-32) + double slack_penalty = 1000.0; ///< R_s weight on slack penalty term. + + // Square-root backward pass (ALTRO Eqs. 19-29) + bool use_sqrt_backward_pass = false; ///< QR-based factorization for numerical robustness. + }; + /** * @brief Main options structure for the CDDP solver. * @@ -210,6 +246,8 @@ namespace cddp IPDDPAlgorithmOptions ipddp; ///< Comprehensive options for the IPDDP solver. MSIPDDPAlgorithmOptions msipddp; ///< Comprehensive options for the MSIPDDP solver. + ALDDPAlgorithmOptions + alddp; ///< Comprehensive options for the ALDDP solver. // Constructor with defaults (relies on member initializers) CDDPOptions() = default; diff --git a/python/src/bind_options.cpp b/python/src/bind_options.cpp index 747e70ab..6779d522 100644 --- a/python/src/bind_options.cpp +++ b/python/src/bind_options.cpp @@ -18,7 +18,8 @@ void bind_options(py::module_& m) { .value("CLDDP", cddp::SolverType::CLDDP) .value("LogDDP", cddp::SolverType::LogDDP) .value("IPDDP", cddp::SolverType::IPDDP) - .value("MSIPDDP", cddp::SolverType::MSIPDDP); + .value("MSIPDDP", cddp::SolverType::MSIPDDP) + .value("ALDDP", cddp::SolverType::ALDDP); // BoxQPOptions py::class_(m, "BoxQPOptions") @@ -103,6 +104,23 @@ void bind_options(py::module_& m) { .def_readwrite("use_controlled_rollout", &cddp::MSIPDDPAlgorithmOptions::use_controlled_rollout) .def_readwrite("costate_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::costate_var_init_scale); + // ALDDPAlgorithmOptions + py::class_(m, "ALDDPOptions") + .def(py::init<>()) + .def_readwrite("lambda_init", &cddp::ALDDPAlgorithmOptions::lambda_init) + .def_readwrite("penalty_init", &cddp::ALDDPAlgorithmOptions::penalty_init) + .def_readwrite("penalty_max", &cddp::ALDDPAlgorithmOptions::penalty_max) + .def_readwrite("penalty_update_factor", &cddp::ALDDPAlgorithmOptions::penalty_update_factor) + .def_readwrite("max_inner_iterations", &cddp::ALDDPAlgorithmOptions::max_inner_iterations) + .def_readwrite("inner_tolerance_init", &cddp::ALDDPAlgorithmOptions::inner_tolerance_init) + .def_readwrite("inner_tolerance_factor", &cddp::ALDDPAlgorithmOptions::inner_tolerance_factor) + .def_readwrite("inner_tolerance_min", &cddp::ALDDPAlgorithmOptions::inner_tolerance_min) + .def_readwrite("max_outer_iterations", &cddp::ALDDPAlgorithmOptions::max_outer_iterations) + .def_readwrite("constraint_tolerance", &cddp::ALDDPAlgorithmOptions::constraint_tolerance) + .def_readwrite("use_boxqp_for_controls", &cddp::ALDDPAlgorithmOptions::use_boxqp_for_controls) + .def_readwrite("slack_penalty", &cddp::ALDDPAlgorithmOptions::slack_penalty) + .def_readwrite("use_sqrt_backward_pass", &cddp::ALDDPAlgorithmOptions::use_sqrt_backward_pass); + // CDDPOptions py::class_(m, "CDDPOptions") .def(py::init<>()) @@ -126,5 +144,6 @@ void bind_options(py::module_& m) { .def_readwrite("filter", &cddp::CDDPOptions::filter) .def_readwrite("log_barrier", &cddp::CDDPOptions::log_barrier) .def_readwrite("ipddp", &cddp::CDDPOptions::ipddp) - .def_readwrite("msipddp", &cddp::CDDPOptions::msipddp); + .def_readwrite("msipddp", &cddp::CDDPOptions::msipddp) + .def_readwrite("alddp", &cddp::CDDPOptions::alddp); } diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 65fde2b3..fd29bc2a 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -11,8 +11,8 @@ namespace py = pybind11; namespace { -constexpr std::array kBuiltinSolverNames = { - "CLDDP", "LogDDP", "IPDDP", "MSIPDDP"}; +constexpr std::array kBuiltinSolverNames = { + "CLDDP", "LogDDP", "IPDDP", "MSIPDDP", "ALDDP"}; bool isBuiltinSolverName(const std::string &solver_name) { for (const char *name : kBuiltinSolverNames) { diff --git a/src/cddp_core/alddp_solver.cpp b/src/cddp_core/alddp_solver.cpp new file mode 100644 index 00000000..c7f23e8e --- /dev/null +++ b/src/cddp_core/alddp_solver.cpp @@ -0,0 +1,1180 @@ +/* + 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/alddp_solver.hpp" +#include "cddp_core/boxqp.hpp" +#include "cddp_core/cddp_core.hpp" +#include +#include +#include +#include +#include +#include + +namespace cddp { + +// ============================================================================ +// Construction / Initialization +// ============================================================================ + +ALDDPSolver::ALDDPSolver() : boxqp_solver_(BoxQPOptions()) {} + +std::string ALDDPSolver::getSolverName() const { return "ALDDP"; } + +void ALDDPSolver::initialize(CDDP &context) { + const CDDPOptions &options = context.getOptions(); + const auto &al_opts = options.alddp; + const int horizon = context.getHorizon(); + const int control_dim = context.getControlDim(); + const int state_dim = context.getStateDim(); + + // Warm start validation + if (options.warm_start) { + bool valid = (k_u_.size() == static_cast(horizon) && + K_u_.size() == static_cast(horizon)); + if (valid && !k_u_.empty()) { + 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) { + valid = false; + break; + } + } + } else { + valid = false; + } + + if (valid) { + if (options.verbose) { + std::cout << "ALDDP: Using warm start with existing solver state" + << std::endl; + } + boxqp_solver_.setOptions(options.box_qp); + if (!context.X_.empty() && !context.U_.empty()) { + computeCost(context); + } + return; + } else if (options.verbose) { + std::cout << "ALDDP: Warning - warm start requested but no valid " + "solver state found. Falling back to cold start." + << std::endl; + } + } + + // Cold start + initializeGains(horizon, control_dim, state_dim); + boxqp_solver_ = BoxQPSolver(options.box_qp); + + // Initialize AL state + al_outer_iter_ = 0; + inner_tolerance_ = al_opts.inner_tolerance_init; + max_constraint_violation_ = 0.0; + prev_max_constraint_violation_ = std::numeric_limits::infinity(); + prev_outer_cost_ = std::numeric_limits::infinity(); + inner_converged_ = false; + inner_iter_count_ = 0; + current_slack_penalty_ = al_opts.slack_penalty; + + // Initialize multipliers and penalties + initializeMultipliersAndPenalties(context); + + // Initialize slack controls for infeasible start + initializeSlackControls(context); + + // Initialize sqrt backward pass workspace + if (al_opts.use_sqrt_backward_pass) { + S_chol_.resize(horizon + 1); + p_.resize(horizon + 1); + for (int t = 0; t <= horizon; ++t) { + S_chol_[t] = Eigen::MatrixXd::Zero(state_dim, state_dim); + p_[t] = Eigen::VectorXd::Zero(state_dim); + } + } + + // Warn if max_iterations may be too low + int effective_budget = al_opts.max_inner_iterations * al_opts.max_outer_iterations; + if (options.max_iterations < effective_budget && options.verbose) { + std::cerr << "ALDDP: Warning - max_iterations (" << options.max_iterations + << ") may be too low for " << al_opts.max_outer_iterations + << " outer x " << al_opts.max_inner_iterations + << " inner iterations (= " << effective_budget << ")" + << std::endl; + } + + if (!context.X_.empty() && !context.U_.empty()) { + computeCost(context); + } +} + +void ALDDPSolver::initializeMultipliersAndPenalties(CDDP &context) { + const auto &al_opts = context.getOptions().alddp; + const int horizon = context.getHorizon(); + + lambda_.clear(); + penalty_.clear(); + G_.clear(); + G_x_.clear(); + G_u_.clear(); + + for (const auto &[name, constraint] : context.getConstraintSet()) { + // Skip ControlConstraint if handled by BoxQP + if (al_opts.use_boxqp_for_controls && name == "ControlConstraint") { + continue; + } + int dual_dim = constraint->getDualDim(); + if (dual_dim == 0) { + dual_dim = constraint->getUpperBound().size(); + } + + lambda_[name].resize(horizon); + penalty_[name].resize(horizon); + G_[name].resize(horizon); + G_x_[name].resize(horizon); + G_u_[name].resize(horizon); + + for (int t = 0; t < horizon; ++t) { + lambda_[name][t] = Eigen::VectorXd::Constant(dual_dim, al_opts.lambda_init); + penalty_[name][t] = Eigen::VectorXd::Constant(dual_dim, al_opts.penalty_init); + } + } + + // Terminal constraints + terminal_lambda_.clear(); + terminal_penalty_.clear(); + G_terminal_.clear(); + + for (const auto &[name, constraint] : context.getTerminalConstraintSet()) { + int dual_dim = constraint->getDualDim(); + if (dual_dim == 0) { + dual_dim = constraint->getUpperBound().size(); + } + terminal_lambda_[name] = Eigen::VectorXd::Constant(dual_dim, al_opts.lambda_init); + terminal_penalty_[name] = Eigen::VectorXd::Constant(dual_dim, al_opts.penalty_init); + } +} + +void ALDDPSolver::initializeSlackControls(CDDP &context) { + const int horizon = context.getHorizon(); + const int state_dim = context.getStateDim(); + + S_.resize(horizon, Eigen::VectorXd::Zero(state_dim)); + k_s_.resize(horizon, Eigen::VectorXd::Zero(state_dim)); + K_s_.resize(horizon, Eigen::MatrixXd::Zero(state_dim, state_dim)); + + // Check if the initial trajectory is dynamically infeasible + double max_slack = 0.0; + for (int t = 0; t < horizon; ++t) { + Eigen::VectorXd x_next = context.getSystem().getDiscreteDynamics( + context.X_[t], context.U_[t], t * context.getTimestep()); + S_[t] = context.X_[t + 1] - x_next; + max_slack = std::max(max_slack, S_[t].lpNorm()); + } + + infeasible_start_ = (max_slack > 1e-8); + if (infeasible_start_ && context.getOptions().verbose) { + std::cout << "ALDDP: Infeasible start detected (max slack = " + << max_slack << "). Using slack controls." << std::endl; + } +} + +// ============================================================================ +// Constraint Evaluation +// ============================================================================ + +void ALDDPSolver::evaluateConstraints(CDDP &context) { + const int horizon = context.getHorizon(); + + // Reset max violation tracker + max_constraint_violation_ = 0.0; + + for (auto &[name, G_vec] : G_) { + const auto &constraint = context.getConstraintSet().at(name); + for (int t = 0; t < horizon; ++t) { + G_vec[t] = constraint->evaluate(context.X_[t], context.U_[t], t); + G_x_[name][t] = constraint->getStateJacobian(context.X_[t], context.U_[t], t); + G_u_[name][t] = constraint->getControlJacobian(context.X_[t], context.U_[t], t); + + // Track max violation using the constraint's own method + double viol = constraint->computeViolation(context.X_[t], context.U_[t], t); + max_constraint_violation_ = std::max(max_constraint_violation_, viol); + } + } + + // Terminal constraints + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + for (auto &[name, G_term] : G_terminal_) { + const auto &constraint = context.getTerminalConstraintSet().at(name); + G_term = constraint->evaluate(context.X_.back(), dummy_u, horizon); + + double viol = constraint->computeViolation(context.X_.back(), dummy_u, horizon); + max_constraint_violation_ = std::max(max_constraint_violation_, viol); + } +} + +double ALDDPSolver::computeMaxConstraintViolation() const { + // max_constraint_violation_ is computed during evaluateConstraints() + return max_constraint_violation_; +} + +double ALDDPSolver::computeMaxSlackNorm() const { + double max_norm = 0.0; + for (const auto &s : S_) { + max_norm = std::max(max_norm, s.lpNorm()); + } + return max_norm; +} + +// ============================================================================ +// AL Cost Derivative Augmentation +// ============================================================================ + +void ALDDPSolver::augmentRunningCostDerivatives( + const CDDP &context, int t, + const Eigen::VectorXd &x, const Eigen::VectorXd &u, + Eigen::VectorXd &l_x, Eigen::VectorXd &l_u, + Eigen::MatrixXd &l_xx, Eigen::MatrixXd &l_uu, + Eigen::MatrixXd &l_ux) const { + + for (const auto &[name, G_vec] : G_) { + const auto &constraint = context.getConstraintSet().at(name); + const Eigen::VectorXd &g = G_vec[t]; + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + const Eigen::VectorXd &lam = lambda_.at(name)[t]; + const Eigen::VectorXd &pen = penalty_.at(name)[t]; + const Eigen::MatrixXd &C_x = G_x_.at(name)[t]; + const Eigen::MatrixXd &C_u = G_u_.at(name)[t]; + + const int dim = g.size(); + for (int i = 0; i < dim; ++i) { + // Violation: c_i = g_i - upper_i (positive means violated) + double c_i = g(i) - upper(i); + + // Determine if this component is active (I_mu masking, Eq. 3) + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + bool is_active = is_equality || (c_i >= 0.0) || (lam(i) > 0.0); + + if (!is_active) continue; + + double effective_lambda = lam(i) + pen(i) * c_i; + + // First-order augmentation + l_x += effective_lambda * C_x.row(i).transpose(); + l_u += effective_lambda * C_u.row(i).transpose(); + + // Second-order augmentation (Gauss-Newton approximation) + l_xx += pen(i) * C_x.row(i).transpose() * C_x.row(i); + l_uu += pen(i) * C_u.row(i).transpose() * C_u.row(i); + l_ux += pen(i) * C_u.row(i).transpose() * C_x.row(i); + } + } +} + +void ALDDPSolver::augmentTerminalCostDerivatives( + const CDDP &context, const Eigen::VectorXd &x_N, + Eigen::VectorXd &V_x, Eigen::MatrixXd &V_xx) const { + + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + + for (const auto &[name, g_term] : G_terminal_) { + const auto &constraint = context.getTerminalConstraintSet().at(name); + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + const Eigen::VectorXd &lam = terminal_lambda_.at(name); + const Eigen::VectorXd &pen = terminal_penalty_.at(name); + const Eigen::MatrixXd C_x = + constraint->getStateJacobian(x_N, dummy_u, context.getHorizon()); + + const int dim = g_term.size(); + for (int i = 0; i < dim; ++i) { + double c_i = g_term(i) - upper(i); + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + bool is_active = is_equality || (c_i >= 0.0) || (lam(i) > 0.0); + + if (!is_active) continue; + + double effective_lambda = lam(i) + pen(i) * c_i; + V_x += effective_lambda * C_x.row(i).transpose(); + V_xx += pen(i) * C_x.row(i).transpose() * C_x.row(i); + } + } +} + +// ============================================================================ +// AL Merit Function (thread-safe, no member writes) +// ============================================================================ + +double ALDDPSolver::computeALMerit( + const CDDP &context, + const std::vector &X, + const std::vector &U, + const std::vector *S_trial) const { + + const auto &al_opts = context.getOptions().alddp; + + // Base cost + double merit = 0.0; + for (int t = 0; t < context.getHorizon(); ++t) { + merit += context.getObjective().running_cost(X[t], U[t], t); + } + merit += context.getObjective().terminal_cost(X.back()); + + // Slack penalty + if (S_trial != nullptr) { + for (int t = 0; t < context.getHorizon(); ++t) { + merit += 0.5 * current_slack_penalty_ * (*S_trial)[t].squaredNorm(); + } + } + + // AL penalty terms for path constraints + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + for (const auto &[name, lam_vec] : lambda_) { + const auto &constraint = context.getConstraintSet().at(name); + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + + for (int t = 0; t < context.getHorizon(); ++t) { + Eigen::VectorXd g = constraint->evaluate(X[t], U[t], t); + const Eigen::VectorXd &lam = lam_vec[t]; + const Eigen::VectorXd &pen = penalty_.at(name)[t]; + + for (int i = 0; i < g.size(); ++i) { + double c_i = g(i) - upper(i); + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + bool is_active = is_equality || (c_i >= 0.0) || (lam(i) > 0.0); + if (is_active) { + merit += lam(i) * c_i + 0.5 * pen(i) * c_i * c_i; + } + } + } + } + + // AL penalty terms for terminal constraints + for (const auto &[name, lam] : terminal_lambda_) { + const auto &constraint = context.getTerminalConstraintSet().at(name); + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + Eigen::VectorXd g = constraint->evaluate(X.back(), dummy_u, context.getHorizon()); + const Eigen::VectorXd &pen = terminal_penalty_.at(name); + + for (int i = 0; i < g.size(); ++i) { + double c_i = g(i) - upper(i); + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + bool is_active = is_equality || (c_i >= 0.0) || (lam(i) > 0.0); + if (is_active) { + merit += lam(i) * c_i + 0.5 * pen(i) * c_i * c_i; + } + } + } + + return merit; +} + +// ============================================================================ +// Backward Pass +// ============================================================================ + +bool ALDDPSolver::backwardPass(CDDP &context) { + if (context.getOptions().alddp.use_sqrt_backward_pass) { + return backwardPassSqrt(context); + } + return backwardPassStandard(context); +} + +bool ALDDPSolver::backwardPassStandard(CDDP &context) { + const CDDPOptions &options = context.getOptions(); + const auto &al_opts = options.alddp; + const int state_dim = context.getStateDim(); + const int control_dim = context.getControlDim(); + const int horizon = context.getHorizon(); + + auto control_constraint = + al_opts.use_boxqp_for_controls + ? context.getConstraint("ControlConstraint") + : nullptr; + + // Terminal cost derivatives with AL augmentation + Eigen::VectorXd V_x = + context.getObjective().getFinalCostGradient(context.X_.back()); + Eigen::MatrixXd V_xx = + context.getObjective().getFinalCostHessian(context.X_.back()); + + augmentTerminalCostDerivatives(context, context.X_.back(), V_x, V_xx); + + // Pre-allocate + Eigen::MatrixXd A(state_dim, state_dim); + Eigen::MatrixXd B(state_dim, 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); + Eigen::MatrixXd Q_ux(control_dim, state_dim); + Eigen::VectorXd k(control_dim); + Eigen::MatrixXd K(control_dim, state_dim); + + dV_ = Eigen::Vector2d::Zero(); + double norm_Vx = V_x.lpNorm<1>(); + double Qu_error = 0.0; + + for (int t = horizon - 1; t >= 0; --t) { + const Eigen::VectorXd &x = context.X_[t]; + const Eigen::VectorXd &u = context.U_[t]; + + const auto [Fx, Fu] = + context.getSystem().getJacobians(x, u, t * context.getTimestep()); + + A = context.getTimestep() * Fx; + A.diagonal().array() += 1.0; + B = context.getTimestep() * Fu; + + auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); + auto [l_xx, l_uu, l_ux] = + context.getObjective().getRunningCostHessians(x, u, t); + + // Augment with AL constraint penalty terms + augmentRunningCostDerivatives(context, t, x, u, l_x, l_u, l_xx, l_uu, l_ux); + + // Slack contribution to V_x through dynamics: x_{t+1} = f(x,u) + s + // The slack penalty 0.5 * R_s * ||s||^2 contributes to cost-to-go + if (infeasible_start_) { + // Slack penalty gradient: R_s * s_t contributes via the identity Jacobian + // Q_s = V_x' + R_s * s_t (handled below after V_x' is available) + // For the standard backward pass, we incorporate slacks after computing V_x, V_xx + } + + 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; + + 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) { + std::cerr << "ALDDP: Q_uu is not positive definite at time " << t + << std::endl; + } + return false; + } + + // Solve for control gains + if (control_constraint == nullptr) { + const Eigen::MatrixXd H = Q_uu_reg.inverse(); + k = -H * Q_u; + K = -H * Q_ux; + } else { + const Eigen::VectorXd lb = control_constraint->rawLowerBound() - u; + const Eigen::VectorXd ub = control_constraint->rawUpperBound() - u; + const Eigen::VectorXd x0 = k_u_[t]; + + BoxQPResult qp_result = boxqp_solver_.solve(Q_uu_reg, Q_u, lb, ub, x0); + + if (qp_result.status == BoxQPStatus::HESSIAN_NOT_PD || + qp_result.status == BoxQPStatus::NO_DESCENT) { + if (options.debug) { + std::cerr << "ALDDP: BoxQP failed at time step " << t << std::endl; + } + return false; + } + + k = qp_result.x; + K = Eigen::MatrixXd::Zero(control_dim, state_dim); + if (qp_result.free.sum() > 0) { + std::vector free_idx; + for (int i = 0; i < control_dim; i++) { + if (qp_result.free(i)) { + 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); + } + } + } + + k_u_[t] = k; + K_u_[t] = K; + + // Solve for slack gains (decoupled, ignoring Q_us/Q_su coupling) + if (infeasible_start_) { + // Q_ss = R_s*I + V_xx (from next step, V_xx is current since we're going backward) + Eigen::MatrixXd Q_ss = current_slack_penalty_ * Eigen::MatrixXd::Identity(state_dim, state_dim) + V_xx; + Eigen::VectorXd Q_s = V_x + current_slack_penalty_ * S_[t]; + Eigen::MatrixXd Q_sx = V_xx * A; + + // Regularize Q_ss + Q_ss.diagonal().array() += context.regularization_; + + Eigen::LDLT ldlt(Q_ss); + k_s_[t] = -ldlt.solve(Q_s); + K_s_[t] = -ldlt.solve(Q_sx); + } + + // Expected cost change + Eigen::Vector2d dV_step; + dV_step << Q_u.dot(k), 0.5 * k.dot(Q_uu * k); + dV_ += dV_step; + + // Add slack contribution to expected cost change + if (infeasible_start_) { + Eigen::MatrixXd Q_ss_unreg = current_slack_penalty_ * Eigen::MatrixXd::Identity(state_dim, state_dim) + V_xx; + Eigen::VectorXd Q_s = V_x + current_slack_penalty_ * S_[t]; + dV_(0) += Q_s.dot(k_s_[t]); + dV_(1) += 0.5 * k_s_[t].dot(Q_ss_unreg * k_s_[t]); + } + + // Update cost-to-go + V_x = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + + K.transpose() * Q_u; + V_xx = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + + K.transpose() * Q_ux; + V_xx = 0.5 * (V_xx + V_xx.transpose()); + + norm_Vx += V_x.lpNorm<1>(); + Qu_error = std::max(Qu_error, Q_u.lpNorm()); + } + + 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; + + return true; +} + +bool ALDDPSolver::backwardPassSqrt(CDDP &context) { + const CDDPOptions &options = context.getOptions(); + const auto &al_opts = options.alddp; + const int state_dim = context.getStateDim(); + const int control_dim = context.getControlDim(); + const int horizon = context.getHorizon(); + + auto control_constraint = + al_opts.use_boxqp_for_controls + ? context.getConstraint("ControlConstraint") + : nullptr; + + // Terminal cost derivatives + Eigen::VectorXd V_x = + context.getObjective().getFinalCostGradient(context.X_.back()); + Eigen::MatrixXd V_xx = + context.getObjective().getFinalCostHessian(context.X_.back()); + + augmentTerminalCostDerivatives(context, context.X_.back(), V_x, V_xx); + + // Regularize terminal Hessian before Cholesky + V_xx.diagonal().array() += 1e-8; + + // Compute Cholesky factor of terminal cost-to-go Hessian: S_N = √P_N + Eigen::LLT llt_terminal(V_xx); + if (llt_terminal.info() != Eigen::Success) { + if (options.debug) { + std::cerr << "ALDDP sqrt: Terminal Hessian Cholesky failed" << std::endl; + } + return false; + } + S_chol_[horizon] = llt_terminal.matrixU(); // upper triangular + p_[horizon] = V_x; + + dV_ = Eigen::Vector2d::Zero(); + double norm_Vx = V_x.lpNorm<1>(); + double Qu_error = 0.0; + + for (int t = horizon - 1; t >= 0; --t) { + const Eigen::VectorXd &x = context.X_[t]; + const Eigen::VectorXd &u = context.U_[t]; + + const auto [Fx, Fu] = + context.getSystem().getJacobians(x, u, t * context.getTimestep()); + + Eigen::MatrixXd A = context.getTimestep() * Fx; + A.diagonal().array() += 1.0; + Eigen::MatrixXd B = context.getTimestep() * Fu; + + auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); + auto [l_xx, l_uu, l_ux] = + context.getObjective().getRunningCostHessians(x, u, t); + + augmentRunningCostDerivatives(context, t, x, u, l_x, l_u, l_xx, l_uu, l_ux); + + const Eigen::MatrixXd &S_next = S_chol_[t + 1]; // upper triangular √P' + const Eigen::VectorXd &p_next = p_[t + 1]; + + // Q-function first-order terms + Eigen::VectorXd Q_x = l_x + A.transpose() * p_next; + Eigen::VectorXd Q_u = l_u + B.transpose() * p_next; + + // Compute Cholesky factors of running cost Hessians + // Add regularization to ensure PD + l_xx.diagonal().array() += context.regularization_; + l_uu.diagonal().array() += context.regularization_; + + Eigen::LLT llt_lxx(l_xx); + Eigen::LLT llt_luu(l_uu); + + if (llt_lxx.info() != Eigen::Success || llt_luu.info() != Eigen::Success) { + if (options.debug) { + std::cerr << "ALDDP sqrt: Running cost Hessian Cholesky failed at t=" + << t << std::endl; + } + return false; + } + + Eigen::MatrixXd sqrt_lxx = llt_lxx.matrixU(); // upper triangular + Eigen::MatrixXd sqrt_luu = llt_luu.matrixU(); + + // Reconstruct V_xx' = S_next^T * S_next for explicit Q-function computation + Eigen::MatrixXd V_xx_next = S_next.transpose() * S_next; + + // Build Q-function Hessians explicitly (use sqrt factors only for propagation) + Eigen::MatrixXd Q_xx = l_xx + A.transpose() * V_xx_next * A; + Eigen::MatrixXd Q_ux = l_ux + B.transpose() * V_xx_next * A; + Eigen::MatrixXd Q_uu = l_uu + B.transpose() * V_xx_next * B; + + // Regularize Q_uu + Eigen::MatrixXd 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) { + std::cerr << "ALDDP sqrt: Q_uu is not positive definite at time " << t + << std::endl; + } + return false; + } + + // Compute Z_uu = cholesky(Q_uu_reg) for gain computation + Eigen::LLT llt_Quu(Q_uu_reg); + if (llt_Quu.info() != Eigen::Success) { + if (options.debug) { + std::cerr << "ALDDP sqrt: Q_uu Cholesky failed at t=" << t << std::endl; + } + return false; + } + Eigen::MatrixXd Z_uu = llt_Quu.matrixU(); // upper triangular + + // Gains via triangular solves (Eqs. 22-23): K = -Z_uu^{-T} Z_uu^{-1} Q_ux + Eigen::VectorXd k_vec(control_dim); + Eigen::MatrixXd K_mat(control_dim, state_dim); + + if (control_constraint == nullptr) { + // d = -solve(Z_uu, solve(Z_uu^T, Q_u)) + Eigen::VectorXd tmp1 = Z_uu.transpose().triangularView().solve(Q_u); + k_vec = -Z_uu.triangularView().solve(tmp1); + + Eigen::MatrixXd tmp2 = Z_uu.transpose().triangularView().solve(Q_ux); + K_mat = -Z_uu.triangularView().solve(tmp2); + } else { + // Reconstruct Q_uu for BoxQP + Eigen::MatrixXd Q_uu_full = Z_uu.transpose() * Z_uu; + + const Eigen::VectorXd lb = control_constraint->rawLowerBound() - u; + const Eigen::VectorXd ub = control_constraint->rawUpperBound() - u; + const Eigen::VectorXd x0 = k_u_[t]; + + BoxQPResult qp_result = boxqp_solver_.solve(Q_uu_full, Q_u, lb, ub, x0); + + if (qp_result.status == BoxQPStatus::HESSIAN_NOT_PD || + qp_result.status == BoxQPStatus::NO_DESCENT) { + if (options.debug) { + std::cerr << "ALDDP sqrt: BoxQP failed at time step " << t << std::endl; + } + return false; + } + + k_vec = qp_result.x; + K_mat = Eigen::MatrixXd::Zero(control_dim, state_dim); + if (qp_result.free.sum() > 0) { + std::vector free_idx; + for (int i = 0; i < control_dim; i++) { + if (qp_result.free(i)) 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_mat.row(free_idx[i]) = K_free.row(i); + } + } + } + + k_u_[t] = k_vec; + K_u_[t] = K_mat; + + // Slack gains (same as standard path) + if (infeasible_start_) { + Eigen::MatrixXd Q_ss = current_slack_penalty_ * Eigen::MatrixXd::Identity(state_dim, state_dim) + + S_next.transpose() * S_next; + Eigen::VectorXd Q_s = p_next + current_slack_penalty_ * S_[t]; + Eigen::MatrixXd Q_sx = S_next.transpose() * S_next * A; + Q_ss.diagonal().array() += context.regularization_; + + Eigen::LDLT ldlt(Q_ss); + k_s_[t] = -ldlt.solve(Q_s); + K_s_[t] = -ldlt.solve(Q_sx); + } + + // Expected cost change (Eq. 25) + Eigen::VectorXd Zuu_d = Z_uu * k_vec; + dV_(0) += Q_u.dot(k_vec); + dV_(1) += 0.5 * Zuu_d.dot(Zuu_d); + + if (infeasible_start_) { + Eigen::MatrixXd Q_ss_unreg = current_slack_penalty_ * Eigen::MatrixXd::Identity(state_dim, state_dim) + + S_next.transpose() * S_next; + Eigen::VectorXd Q_s = p_next + current_slack_penalty_ * S_[t]; + dV_(0) += Q_s.dot(k_s_[t]); + dV_(1) += 0.5 * k_s_[t].dot(Q_ss_unreg * k_s_[t]); + } + + // Cost-to-go gradient update (standard Riccati) + p_[t] = Q_x + K_mat.transpose() * Q_uu * k_vec + Q_ux.transpose() * k_vec + + K_mat.transpose() * Q_u; + + // Cost-to-go Hessian update (standard Riccati, then take Cholesky for sqrt storage) + Eigen::MatrixXd V_xx_new = Q_xx + K_mat.transpose() * Q_uu * K_mat + + Q_ux.transpose() * K_mat + K_mat.transpose() * Q_ux; + V_xx_new = 0.5 * (V_xx_new + V_xx_new.transpose()); + V_xx_new.diagonal().array() += 1e-8; + + Eigen::LLT llt_Vxx(V_xx_new); + if (llt_Vxx.info() != Eigen::Success) { + if (options.debug) { + std::cerr << "ALDDP sqrt: V_xx Cholesky failed at t=" << t << std::endl; + } + return false; + } + S_chol_[t] = llt_Vxx.matrixU(); + + norm_Vx += p_[t].lpNorm<1>(); + Qu_error = std::max(Qu_error, Q_u.lpNorm()); + } + + 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; + + return true; +} + +// ============================================================================ +// Forward Pass +// ============================================================================ + +ForwardPassResult ALDDPSolver::forwardPass(CDDP &context, double alpha_pr) { + ForwardPassResult result; + result.success = false; + result.cost = std::numeric_limits::infinity(); + result.merit_function = std::numeric_limits::infinity(); + result.alpha_pr = alpha_pr; + + result.state_trajectory = context.X_; + result.control_trajectory = context.U_; + result.state_trajectory[0] = context.getInitialState(); + + const auto &al_opts = context.getOptions().alddp; + auto control_constraint = + al_opts.use_boxqp_for_controls + ? context.getConstraint("ControlConstraint") + : nullptr; + + // Trial slack controls (local copy for thread safety) + std::vector S_trial; + if (infeasible_start_) { + S_trial = S_; + } + + double J_new = 0.0; + + for (int t = 0; t < context.getHorizon(); ++t) { + const Eigen::VectorXd &x = result.state_trajectory[t]; + const Eigen::VectorXd delta_x = x - context.X_[t]; + + result.control_trajectory[t] = + result.control_trajectory[t] + alpha_pr * k_u_[t] + K_u_[t] * delta_x; + + if (control_constraint != nullptr) { + result.control_trajectory[t] = + control_constraint->clamp(result.control_trajectory[t]); + } + + J_new += context.getObjective().running_cost(x, result.control_trajectory[t], t); + + // Dynamics with slack + result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( + x, result.control_trajectory[t], t * context.getTimestep()); + + if (infeasible_start_) { + S_trial[t] = S_[t] + alpha_pr * k_s_[t] + K_s_[t] * delta_x; + result.state_trajectory[t + 1] += S_trial[t]; + } + } + + J_new += context.getObjective().terminal_cost(result.state_trajectory.back()); + + // Compute AL merit function (thread-safe, uses local S_trial) + double merit_new = computeALMerit( + context, result.state_trajectory, result.control_trajectory, + infeasible_start_ ? &S_trial : nullptr); + + // Armijo condition on the AL merit function + double current_merit = context.merit_function_; + double dJ = current_merit - merit_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); + + result.success = reduction_ratio > context.getOptions().filter.armijo_constant; + result.cost = J_new; + result.merit_function = merit_new; + + // Compute constraint violation for this trial (using local evaluation) + double max_viol = 0.0; + for (const auto &[name, lam_vec] : lambda_) { + const auto &constraint = context.getConstraintSet().at(name); + for (int t = 0; t < context.getHorizon(); ++t) { + double viol = constraint->computeViolation( + result.state_trajectory[t], result.control_trajectory[t], t); + max_viol = std::max(max_viol, viol); + } + } + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + for (const auto &[name, lam] : terminal_lambda_) { + const auto &constraint = context.getTerminalConstraintSet().at(name); + double viol = constraint->computeViolation( + result.state_trajectory.back(), dummy_u, context.getHorizon()); + max_viol = std::max(max_viol, viol); + } + result.constraint_violation = max_viol; + + // Stash slack trajectory for applyForwardPassResult + if (infeasible_start_ && result.success) { + // Store in the optional dynamics_trajectory field + result.dynamics_trajectory = S_trial; + } + + return result; +} + +void ALDDPSolver::applyForwardPassResult(CDDP &context, + const ForwardPassResult &result) { + // Apply base class updates (X_, U_, cost_, merit_function_, alpha_pr_) + CDDPSolverBase::applyForwardPassResult(context, result); + + // Update merit to AL merit (not bare cost) + context.merit_function_ = result.merit_function; + + // Update slack controls if infeasible start + if (infeasible_start_ && result.dynamics_trajectory.has_value()) { + S_ = result.dynamics_trajectory.value(); + } +} + +// ============================================================================ +// Iteration Setup and Convergence +// ============================================================================ + +void ALDDPSolver::preIterationSetup(CDDP &context) { + evaluateConstraints(context); + // max_constraint_violation_ is set by evaluateConstraints() + + // Set initial merit function to AL merit + context.merit_function_ = computeALMerit( + context, context.X_, context.U_, + infeasible_start_ ? &S_ : nullptr); +} + +bool ALDDPSolver::checkEarlyConvergence(CDDP &context, int /*iter*/, + std::string &reason) { + // Check if already fully converged after backward pass + const auto &al_opts = context.getOptions().alddp; + + if (context.inf_du_ < al_opts.constraint_tolerance && + max_constraint_violation_ < al_opts.constraint_tolerance && + (!infeasible_start_ || computeMaxSlackNorm() < al_opts.constraint_tolerance)) { + reason = "OptimalSolutionFound"; + return true; + } + return false; +} + +bool ALDDPSolver::checkConvergence(CDDP &context, double dJ, double /*dL*/, + int /*iter*/, std::string &reason) { + const auto &al_opts = context.getOptions().alddp; + + // Check for AL outer iteration limit + if (al_outer_iter_ >= al_opts.max_outer_iterations) { + reason = "MaxOuterIterationsReached"; + return true; + } + + // Check full convergence (outer) + bool constraints_satisfied = max_constraint_violation_ < al_opts.constraint_tolerance; + bool slacks_zero = !infeasible_start_ || computeMaxSlackNorm() < al_opts.constraint_tolerance; + bool dual_converged = context.inf_du_ < al_opts.constraint_tolerance; + + if (constraints_satisfied && slacks_zero && dual_converged) { + reason = "OptimalSolutionFound"; + return true; + } + + // Check inner convergence + inner_iter_count_++; + bool inner_cost_converged = (dJ > 0.0 && dJ < context.getOptions().acceptable_tolerance); + bool inner_du_converged = context.inf_du_ < inner_tolerance_; + bool inner_iter_limit = inner_iter_count_ >= al_opts.max_inner_iterations; + + if (inner_cost_converged || inner_du_converged || inner_iter_limit) { + inner_converged_ = true; + // Return false so postIterationUpdate can handle the AL outer update + } + + return false; +} + +// ============================================================================ +// AL Outer Loop (Multiplier/Penalty Updates) +// ============================================================================ + +void ALDDPSolver::postIterationUpdate(CDDP &context, bool /*forward_pass_success*/) { + if (!inner_converged_) return; + + const auto &al_opts = context.getOptions().alddp; + + // Re-evaluate constraints at current trajectory + evaluateConstraints(context); + // max_constraint_violation_ is set by evaluateConstraints() + double current_violation = max_constraint_violation_; + + // Update multipliers (Eq. 17) + updateMultipliers(context); + + // Check if violation improved sufficiently + if (current_violation > 0.5 * prev_max_constraint_violation_ || al_outer_iter_ == 0) { + // Insufficient progress -- increase penalties + updatePenalties(context); + } + + // Increase slack penalty + if (infeasible_start_ && computeMaxSlackNorm() > al_opts.constraint_tolerance) { + current_slack_penalty_ *= al_opts.penalty_update_factor; + current_slack_penalty_ = std::min(current_slack_penalty_, al_opts.penalty_max); + } + + // Cost divergence check + if (context.cost_ > 100.0 * prev_outer_cost_ && al_outer_iter_ > 0) { + if (context.getOptions().verbose) { + std::cerr << "ALDDP: Cost divergence detected (cost = " << context.cost_ + << ", prev = " << prev_outer_cost_ << ")" << std::endl; + } + } + + // Update state + prev_max_constraint_violation_ = current_violation; + prev_outer_cost_ = context.cost_; + al_outer_iter_++; + inner_converged_ = false; + inner_iter_count_ = 0; + + // Tighten inner tolerance + inner_tolerance_ = std::max( + al_opts.inner_tolerance_min, + inner_tolerance_ * al_opts.inner_tolerance_factor); + + // Reset regularization for new inner loop + context.regularization_ = context.getOptions().regularization.initial_value; + + if (context.getOptions().verbose) { + std::cout << " ALDDP outer iter " << al_outer_iter_ + << ": violation=" << std::scientific << std::setprecision(2) + << current_violation + << " slack=" << computeMaxSlackNorm() + << " inner_tol=" << inner_tolerance_ + << std::endl; + } +} + +void ALDDPSolver::updateMultipliers(CDDP &context) { + // Path constraints (Eq. 17) + for (auto &[name, lam_vec] : lambda_) { + const auto &constraint = context.getConstraintSet().at(name); + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + + for (int t = 0; t < context.getHorizon(); ++t) { + const Eigen::VectorXd &g = G_[name][t]; + for (int i = 0; i < g.size(); ++i) { + double c_i = g(i) - upper(i); + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + if (is_equality) { + lam_vec[t](i) += penalty_[name][t](i) * c_i; + } else { + lam_vec[t](i) = std::max(0.0, lam_vec[t](i) + penalty_[name][t](i) * c_i); + } + } + } + } + + // Terminal constraints + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + for (auto &[name, lam] : terminal_lambda_) { + const auto &constraint = context.getTerminalConstraintSet().at(name); + const Eigen::VectorXd &upper = constraint->getUpperBound(); + const Eigen::VectorXd &lower = constraint->getLowerBound(); + const Eigen::VectorXd &g = G_terminal_[name]; + + for (int i = 0; i < g.size(); ++i) { + double c_i = g(i) - upper(i); + bool is_equality = (std::abs(lower(i) - upper(i)) < 1e-12); + if (is_equality) { + lam(i) += terminal_penalty_[name](i) * c_i; + } else { + lam(i) = std::max(0.0, lam(i) + terminal_penalty_[name](i) * c_i); + } + } + } +} + +void ALDDPSolver::updatePenalties(CDDP &context) { + const auto &al_opts = context.getOptions().alddp; + + for (auto &[name, pen_vec] : penalty_) { + for (int t = 0; t < context.getHorizon(); ++t) { + pen_vec[t] *= al_opts.penalty_update_factor; + pen_vec[t] = pen_vec[t].cwiseMin(al_opts.penalty_max); + } + } + + for (auto &[name, pen] : terminal_penalty_) { + pen *= al_opts.penalty_update_factor; + pen = pen.cwiseMin(al_opts.penalty_max); + } +} + +// ============================================================================ +// Regularization Limit Handler +// ============================================================================ + +bool ALDDPSolver::handleBackwardPassRegularizationLimit( + CDDP &context, std::string &termination_reason) { + const auto &al_opts = context.getOptions().alddp; + + // Perform AL outer update in-place since postIterationUpdate won't fire + evaluateConstraints(context); + double current_violation = computeMaxConstraintViolation(); + + // Check if truly converged + if (current_violation < al_opts.constraint_tolerance && + (!infeasible_start_ || computeMaxSlackNorm() < al_opts.constraint_tolerance)) { + termination_reason = "OptimalSolutionFound"; + return true; + } + + // Update multipliers and penalties + updateMultipliers(context); + if (current_violation > 0.5 * prev_max_constraint_violation_ || al_outer_iter_ == 0) { + updatePenalties(context); + } + + prev_max_constraint_violation_ = current_violation; + max_constraint_violation_ = current_violation; + al_outer_iter_++; + + if (al_outer_iter_ < al_opts.max_outer_iterations) { + // Reset regularization and return true (early exit with current best) + context.regularization_ = context.getOptions().regularization.initial_value; + termination_reason = "RegularizationLimitReached"; + return true; + } + + termination_reason = "MaxOuterIterationsReached"; + return false; +} + +// ============================================================================ +// History and Solution +// ============================================================================ + +void ALDDPSolver::recordIterationHistory(const CDDP &context) { + CDDPSolverBase::recordIterationHistory(context); + // AL-specific history could be added here + history_.primal_infeasibility.push_back(max_constraint_violation_); +} + +void ALDDPSolver::populateSolverSpecificSolution(CDDPSolution &solution, + const CDDP &context) { + solution.final_primal_infeasibility = max_constraint_violation_; + solution.final_dual_infeasibility = context.inf_du_; + + // Append AL info to status message + std::string al_info = " (" + std::to_string(al_outer_iter_) + " AL outer iters)"; + solution.status_message += al_info; +} + +// ============================================================================ +// Printing +// ============================================================================ + +void ALDDPSolver::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(10) + << "violation" << " " << std::setw(8) << "lg(rg)" << " " + << std::setw(8) << "alpha" << " " << std::setw(5) << "outer" + << std::endl; + } + + std::cout << std::setw(4) << iter << " " << std::setw(12) << std::scientific + << std::setprecision(4) << context.cost_ << " " << std::setw(10) + << std::scientific << std::setprecision(2) << context.inf_du_ + << " " << std::setw(10) << std::scientific << std::setprecision(2) + << max_constraint_violation_ << " " << 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(5) << al_outer_iter_ + << std::endl; +} + +void ALDDPSolver::printSolutionSummary(const CDDPSolution &solution) const { + std::cout << "\n=== ALDDP Solution Summary ===" << std::endl; + std::cout << "Status: " << solution.status_message << std::endl; + std::cout << "Iterations: " << solution.iterations_completed << std::endl; + std::cout << "Solve time: " << std::fixed << std::setprecision(2) + << solution.solve_time_ms << " ms" << std::endl; + std::cout << "Final objective: " << std::scientific << std::setprecision(6) + << solution.final_objective << std::endl; + std::cout << "Primal infeasibility: " << std::scientific << std::setprecision(2) + << solution.final_primal_infeasibility << std::endl; + std::cout << "Dual infeasibility: " << std::scientific << std::setprecision(2) + << solution.final_dual_infeasibility << std::endl; + std::cout << "==============================\n" << std::endl; +} + +} // namespace cddp diff --git a/src/cddp_core/cddp_core.cpp b/src/cddp_core/cddp_core.cpp index 65a3cf8c..b32544a7 100644 --- a/src/cddp_core/cddp_core.cpp +++ b/src/cddp_core/cddp_core.cpp @@ -19,6 +19,7 @@ #include "cddp_core/ipddp_solver.hpp" // For IPDDPSolver #include "cddp_core/logddp_solver.hpp" // For LogDDPSolver #include "cddp_core/msipddp_solver.hpp" // For MSIPDDPSolver +#include "cddp_core/alddp_solver.hpp" // For ALDDPSolver #include "cddp_core/options.hpp" // For CDDPOptions structure #include // For std::min, std::max #include @@ -262,6 +263,8 @@ std::string solverTypeToString(SolverType solver_type) { return "IPDDP"; case SolverType::MSIPDDP: return "MSIPDDP"; + case SolverType::ALDDP: + return "ALDDP"; default: return "CLDDP"; // Default fallback } @@ -290,6 +293,8 @@ CDDP::createSolver(const std::string &solver_type) { return std::make_unique(); } else if (solver_type == "MSIPDDP") { return std::make_unique(); + } else if (solver_type == "ALDDP") { + return std::make_unique(); } return nullptr; // Solver not found @@ -321,7 +326,7 @@ CDDPSolution CDDP::solve(const std::string &solver_type) { for (const auto &name : available) { std::cout << name << " "; } - std::cout << "CLDDP LogDDP IPDDP MSIPDDP" << std::endl; + std::cout << "CLDDP LogDDP IPDDP MSIPDDP ALDDP" << std::endl; } return solution; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 68fb1003..5cc4ca24 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -143,6 +143,10 @@ add_executable(test_msipddp_solver cddp_core/test_msipddp_solver.cpp) target_link_libraries(test_msipddp_solver gtest gmock gtest_main cddp) gtest_discover_tests(test_msipddp_solver) +add_executable(test_alddp_solver cddp_core/test_alddp_solver.cpp) +target_link_libraries(test_alddp_solver gtest gmock gtest_main cddp) +gtest_discover_tests(test_alddp_solver) + # add_executable(test_logcddp_core cddp_core/test_logcddp_core.cpp) # target_link_libraries(test_logcddp_core gtest gmock gtest_main cddp) # gtest_discover_tests(test_logcddp_core) diff --git a/tests/cddp_core/test_alddp_solver.cpp b/tests/cddp_core/test_alddp_solver.cpp new file mode 100644 index 00000000..b5a413a7 --- /dev/null +++ b/tests/cddp_core/test_alddp_solver.cpp @@ -0,0 +1,599 @@ +/* + 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 +#include +#include + +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +#include "cddp.hpp" + +// Helper: create CDDP options for ALDDP tests +static cddp::CDDPOptions makeALDDPOptions() { + cddp::CDDPOptions options; + options.max_iterations = 500; + options.tolerance = 1e-3; + options.acceptable_tolerance = 1e-4; + options.verbose = true; + options.debug = false; + + options.alddp.penalty_init = 1.0; + options.alddp.penalty_max = 1e6; + options.alddp.penalty_update_factor = 10.0; + options.alddp.max_inner_iterations = 30; + options.alddp.max_outer_iterations = 15; + options.alddp.constraint_tolerance = 1e-4; + options.alddp.inner_tolerance_init = 1e-2; + options.alddp.use_sqrt_backward_pass = false; + return options; +} + +// ============================================================================ +// CartPole tests +// ============================================================================ + +TEST(ALDDPTest, SolveCartPole) { + // CartPole swing-up with control bounds only (BoxQP path) + const int state_dim = 4; + const int control_dim = 1; + const int horizon = 200; + const double timestep = 0.02; + + auto system = std::make_unique(timestep, "rk4"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state << 0.0, M_PI, 0.0, 0.0; // Cart at origin, pole upright + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + // Cart at origin, pole down + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + // Control bounds + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -20.0; + u_ub << 20.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto options = makeALDDPOptions(); + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_TRUE(solution.status_message.find("Optimal") != std::string::npos || + solution.status_message.find("Acceptable") != std::string::npos || + solution.status_message.find("RegularizationLimit") != std::string::npos); + EXPECT_GT(solution.iterations_completed, 0); + + // Check that final state is near goal + const auto &x_final = solution.state_trajectory.back(); + EXPECT_NEAR(x_final(1), M_PI, 0.5); // Pole angle near upright +} + +TEST(ALDDPTest, SolveCartPoleWithStateBounds) { + // CartPole with state constraints (cart position limits) + const int state_dim = 4; + const int control_dim = 1; + const int horizon = 200; + const double timestep = 0.02; + + auto system = std::make_unique(timestep, "rk4"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state << 0.0, M_PI, 0.0, 0.0; + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + // Control bounds + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -20.0; + u_ub << 20.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + // State bounds: cart position [-2, 2] + Eigen::VectorXd x_lb(state_dim), x_ub(state_dim); + x_lb << -2.0, -1e6, -1e6, -1e6; + x_ub << 2.0, 1e6, 1e6, 1e6; + solver.addPathConstraint("StateConstraint", + std::make_unique(x_lb, x_ub)); + + auto options = makeALDDPOptions(); + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(solution.iterations_completed, 0); + + // Verify cart position stays within bounds (with some tolerance) + for (const auto &x : solution.state_trajectory) { + EXPECT_GE(x(0), -2.1); + EXPECT_LE(x(0), 2.1); + } +} + +// ============================================================================ +// Unicycle tests +// ============================================================================ + +TEST(ALDDPTest, SolveUnicycle) { + // Unicycle path following with control bounds + const int state_dim = 3; // x, y, theta + const int control_dim = 2; // v, omega + const int horizon = 100; + const double timestep = 0.1; + + auto system = std::make_unique(timestep, "euler"); + + Eigen::MatrixXd Q = 0.1 * Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state(state_dim); + goal_state << 5.0, 5.0, 0.0; + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + // Control bounds + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -2.0, -2.0; + u_ub << 2.0, 2.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto options = makeALDDPOptions(); + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(solution.iterations_completed, 0); + + // Check that final state approaches goal + const auto &x_final = solution.state_trajectory.back(); + EXPECT_NEAR(x_final(0), 5.0, 1.0); + EXPECT_NEAR(x_final(1), 5.0, 1.0); +} + +TEST(ALDDPTest, SolveUnicycleWithStateBounds) { + // Unicycle with state constraints (position bounds) + const int state_dim = 3; + const int control_dim = 2; + const int horizon = 100; + const double timestep = 0.1; + + auto system = std::make_unique(timestep, "euler"); + + Eigen::MatrixXd Q = 0.1 * Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state(state_dim); + goal_state << 5.0, 5.0, 0.0; + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -2.0, -2.0; + u_ub << 2.0, 2.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + // State bounds: y position [0, 8] + Eigen::VectorXd x_lb(state_dim), x_ub(state_dim); + x_lb << -1e6, 0.0, -1e6; + x_ub << 1e6, 8.0, 1e6; + solver.addPathConstraint("StateConstraint", + std::make_unique(x_lb, x_ub)); + + auto options = makeALDDPOptions(); + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(solution.iterations_completed, 0); + + // Verify y-position stays within bounds + for (const auto &x : solution.state_trajectory) { + EXPECT_GE(x(1), -0.2); + EXPECT_LE(x(1), 8.2); + } +} + +// ============================================================================ +// Car parking with obstacles +// ============================================================================ + +TEST(ALDDPTest, SolveCarParking) { + // Car parallel parking with control bounds + const int state_dim = 4; // x, y, theta, v + const int control_dim = 2; // acceleration, steering rate + const int horizon = 100; + const double timestep = 0.1; + + auto system = std::make_unique(timestep, 2.0, "rk4"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state(state_dim); + goal_state << 5.0, 2.0, 0.0, 0.0; // Park at (5,2), heading=0, stopped + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -3.0, -1.0; + u_ub << 3.0, 1.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto options = makeALDDPOptions(); + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(solution.iterations_completed, 0); +} + +// ============================================================================ +// Infeasible start +// ============================================================================ + +TEST(ALDDPTest, SolveWithInfeasibleStart) { + // CartPole with interpolated (infeasible) state trajectory + const int state_dim = 4; + const int control_dim = 1; + const int horizon = 200; + const double timestep = 0.02; + + auto system = std::make_unique(timestep, "rk4"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state << 0.0, M_PI, 0.0, 0.0; + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -20.0; + u_ub << 20.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + // Create an interpolated (dynamically infeasible) initial trajectory + std::vector X_init(horizon + 1); + for (int t = 0; t <= horizon; ++t) { + double alpha = static_cast(t) / horizon; + X_init[t] = (1.0 - alpha) * initial_state + alpha * goal_state; + } + + auto options = makeALDDPOptions(); + options.alddp.slack_penalty = 100.0; + solver.setOptions(options); + solver.setInitialTrajectory( + X_init, + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(solution.iterations_completed, 0); + // The solver should produce a valid trajectory + EXPECT_FALSE(solution.state_trajectory.empty()); +} + +// ============================================================================ +// Sqrt backward pass comparison +// ============================================================================ + +TEST(ALDDPTest, SolveSqrtBackwardPass) { + // Same unicycle problem, compare sqrt vs standard backward pass + const int state_dim = 3; + const int control_dim = 2; + const int horizon = 50; + const double timestep = 0.1; + + Eigen::VectorXd goal_state(state_dim); + goal_state << 3.0, 3.0, 0.0; + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + Eigen::MatrixXd Q = 0.1 * Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + std::vector empty_ref; + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -2.0, -2.0; + u_ub << 2.0, 2.0; + + // Standard backward pass + double cost_standard; + { + auto system = std::make_unique(timestep, "euler"); + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto opts = makeALDDPOptions(); + opts.alddp.use_sqrt_backward_pass = false; + opts.max_iterations = 200; + opts.verbose = false; + opts.print_solver_header = false; + solver.setOptions(opts); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + auto sol = solver.solve(cddp::SolverType::ALDDP); + cost_standard = sol.final_objective; + } + + // Sqrt backward pass + double cost_sqrt; + { + auto system = std::make_unique(timestep, "euler"); + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto opts = makeALDDPOptions(); + opts.alddp.use_sqrt_backward_pass = true; + opts.max_iterations = 200; + opts.max_cpu_time = 5.0; // 5 second timeout + opts.verbose = false; + opts.print_solver_header = false; + solver.setOptions(opts); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + auto sol = solver.solve(cddp::SolverType::ALDDP); + cost_sqrt = sol.final_objective; + } + + // Both should produce finite costs + EXPECT_TRUE(std::isfinite(cost_standard)); + EXPECT_TRUE(std::isfinite(cost_sqrt)); +} + +// ============================================================================ +// Convergence failure +// ============================================================================ + +TEST(ALDDPTest, ConvergenceFailure) { + // Overly constrained problem -- should terminate gracefully + const int state_dim = 3; + const int control_dim = 2; + const int horizon = 50; + const double timestep = 0.1; + + auto system = std::make_unique(timestep, "euler"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state(state_dim); + goal_state << 100.0, 100.0, 0.0; // Very far goal + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + // Very tight control bounds + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -0.1, -0.1; + u_ub << 0.1, 0.1; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + // Very tight state bounds (impossible to reach goal) + Eigen::VectorXd x_lb(state_dim), x_ub(state_dim); + x_lb << -1.0, -1.0, -1e6; + x_ub << 1.0, 1.0, 1e6; + solver.addPathConstraint("StateConstraint", + std::make_unique(x_lb, x_ub)); + + auto options = makeALDDPOptions(); + options.alddp.max_outer_iterations = 3; + options.max_iterations = 100; + options.verbose = false; + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + cddp::CDDPSolution solution = solver.solve(cddp::SolverType::ALDDP); + + // Should terminate, not crash + EXPECT_FALSE(solution.state_trajectory.empty()); +} + +// ============================================================================ +// Constraint-free comparison with CLDDP +// ============================================================================ + +TEST(ALDDPTest, ConstraintFreeMatchesCLDDP) { + // Same problem solved by ALDDP and CLDDP should give similar results + const int state_dim = 4; + const int control_dim = 1; + const int horizon = 100; + const double timestep = 0.02; + + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state << 0.0, M_PI, 0.0, 0.0; + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + std::vector empty_ref; + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -20.0; + u_ub << 20.0; + + // CLDDP solve + double cost_clddp; + { + auto system = std::make_unique(timestep, "rk4"); + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + cddp::CDDPOptions opts; + opts.max_iterations = 200; + opts.tolerance = 1e-4; + opts.acceptable_tolerance = 1e-5; + opts.verbose = false; + opts.print_solver_header = false; + solver.setOptions(opts); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + auto sol = solver.solve(cddp::SolverType::CLDDP); + cost_clddp = sol.final_objective; + } + + // ALDDP solve (no AL constraints, only BoxQP control bounds) + double cost_alddp; + { + auto system = std::make_unique(timestep, "rk4"); + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto opts = makeALDDPOptions(); + opts.verbose = false; + opts.print_solver_header = false; + solver.setOptions(opts); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + auto sol = solver.solve(cddp::SolverType::ALDDP); + cost_alddp = sol.final_objective; + } + + // Costs should be in the same ballpark (within 50%) + if (cost_clddp > 0 && cost_alddp > 0) { + double ratio = cost_alddp / cost_clddp; + EXPECT_GT(ratio, 0.5); + EXPECT_LT(ratio, 2.0); + } +} From 39604d3e529bdc0ce1246074b6d940175514e1b7 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Fri, 3 Apr 2026 11:55:01 -0400 Subject: [PATCH 14/14] Fix warm-start handling in constrained solvers Reuse solver state across repeated constrained solves so warm starts can keep their gains, and refresh ALDDP slack controls when a new trajectory seed is supplied. Simplify LogDDP barrier evaluation for single-shooting mode and add regression tests for reseeded trajectories and invalid rollout options. --- include/cddp-cpp/cddp_core/barrier.hpp | 24 +- include/cddp-cpp/cddp_core/logddp_solver.hpp | 25 +- src/cddp_core/alddp_solver.cpp | 5 + src/cddp_core/cddp_core.cpp | 35 +- src/cddp_core/logddp_solver.cpp | 649 ++++++------------- tests/cddp_core/test_alddp_solver.cpp | 61 ++ tests/cddp_core/test_logddp_solver.cpp | 116 +++- 7 files changed, 419 insertions(+), 496 deletions(-) diff --git a/include/cddp-cpp/cddp_core/barrier.hpp b/include/cddp-cpp/cddp_core/barrier.hpp index df517760..34f46ad8 100644 --- a/include/cddp-cpp/cddp_core/barrier.hpp +++ b/include/cddp-cpp/cddp_core/barrier.hpp @@ -59,8 +59,8 @@ class RelaxedLogBarrier { * @return Barrier function value. */ double evaluate(const Constraint &constraint, const Eigen::VectorXd &state, - const Eigen::VectorXd &control) const { - Eigen::VectorXd g_val = constraint.evaluate(state, control); + const Eigen::VectorXd &control, int index = 0) const { + Eigen::VectorXd g_val = constraint.evaluate(state, control, index); Eigen::VectorXd L = constraint.getLowerBound(); Eigen::VectorXd U = constraint.getUpperBound(); int constraint_dim = g_val.size(); @@ -99,13 +99,13 @@ class RelaxedLogBarrier { */ std::tuple getGradients(const Constraint &constraint, const Eigen::VectorXd &state, - const Eigen::VectorXd &control) const { + const Eigen::VectorXd &control, int index = 0) const { - Eigen::VectorXd g_val = constraint.evaluate(state, control); + Eigen::VectorXd g_val = constraint.evaluate(state, control, index); Eigen::VectorXd L = constraint.getLowerBound(); Eigen::VectorXd U = constraint.getUpperBound(); - Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control); - Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control); + Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control, index); + Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control, index); int state_dim = state.size(); int control_dim = control.size(); @@ -151,13 +151,13 @@ class RelaxedLogBarrier { */ std::tuple getHessians(const Constraint &constraint, const Eigen::VectorXd &state, - const Eigen::VectorXd &control) const { + const Eigen::VectorXd &control, int index = 0) const { - Eigen::VectorXd g_val = constraint.evaluate(state, control); + Eigen::VectorXd g_val = constraint.evaluate(state, control, index); Eigen::VectorXd L = constraint.getLowerBound(); Eigen::VectorXd U = constraint.getUpperBound(); - Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control); - Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control); + Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control, index); + Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control, index); int state_dim = state.size(); int control_dim = control.size(); @@ -171,7 +171,7 @@ class RelaxedLogBarrier { Gux_constraint_vec; bool constraint_provides_hessians = true; try { - auto hess_tuple = constraint.getHessians(state, control); + auto hess_tuple = constraint.getHessians(state, control, index); Gxx_constraint_vec = std::get<0>(hess_tuple); Guu_constraint_vec = std::get<1>(hess_tuple); Gux_constraint_vec = std::get<2>(hess_tuple); @@ -536,4 +536,4 @@ class DiscreteBarrierState { } // namespace cddp -#endif // CDDP_BARRIER_HPP \ No newline at end of file +#endif // CDDP_BARRIER_HPP diff --git a/include/cddp-cpp/cddp_core/logddp_solver.hpp b/include/cddp-cpp/cddp_core/logddp_solver.hpp index 9c209f3f..f0df6303 100644 --- a/include/cddp-cpp/cddp_core/logddp_solver.hpp +++ b/include/cddp-cpp/cddp_core/logddp_solver.hpp @@ -56,12 +56,6 @@ class LogDDPSolver : public CDDPSolverBase { void printIteration(int iter, const CDDP &context) const override; private: - // Dynamics storage (forward-simulated trajectory) - std::vector F_; - - // Constraint values g(x,u) - g_ub - std::map> G_; - // Log-barrier method std::unique_ptr relaxed_log_barrier_; double mu_; @@ -70,9 +64,6 @@ class LogDDPSolver : public CDDPSolverBase { // Filter-based line search double constraint_violation_; - // Multi-shooting parameters - int ms_segment_length_; - /** * @brief Evaluate trajectory by computing cost, dynamics, and merit function. */ @@ -82,6 +73,22 @@ class LogDDPSolver : public CDDPSolverBase { * @brief Reset/initialize the filter for line search. */ void resetFilter(CDDP &context); + + void augmentRunningCostDerivatives( + const CDDP &context, int t, const Eigen::VectorXd &x, + const Eigen::VectorXd &u, Eigen::VectorXd &l_x, Eigen::VectorXd &l_u, + Eigen::MatrixXd &l_xx, Eigen::MatrixXd &l_uu, + Eigen::MatrixXd &l_ux) const; + + void augmentTerminalCostDerivatives(const CDDP &context, + const Eigen::VectorXd &x_N, + Eigen::VectorXd &V_x, + Eigen::MatrixXd &V_xx) const; + + double computeBarrierMerit(const CDDP &context, + const std::vector &X, + const std::vector &U, + double *max_constraint_violation = nullptr) const; }; } // namespace cddp diff --git a/src/cddp_core/alddp_solver.cpp b/src/cddp_core/alddp_solver.cpp index c7f23e8e..7f62ccaf 100644 --- a/src/cddp_core/alddp_solver.cpp +++ b/src/cddp_core/alddp_solver.cpp @@ -63,6 +63,11 @@ void ALDDPSolver::initialize(CDDP &context) { << std::endl; } boxqp_solver_.setOptions(options.box_qp); + // The solver state (gains, multipliers, penalties) is reused, but the + // current seeded trajectory may have changed since the last solve. + // Refresh slack controls so warm starts remain consistent with the + // current X/U pair, including dynamically infeasible reseeds. + initializeSlackControls(context); if (!context.X_.empty() && !context.U_.empty()) { computeCost(context); } diff --git a/src/cddp_core/cddp_core.cpp b/src/cddp_core/cddp_core.cpp index b32544a7..bfbcb451 100644 --- a/src/cddp_core/cddp_core.cpp +++ b/src/cddp_core/cddp_core.cpp @@ -269,6 +269,26 @@ std::string solverTypeToString(SolverType solver_type) { return "CLDDP"; // Default fallback } } + +std::string canonicalizeSolverType(const std::string &solver_type) { + if (solver_type == "CLCDDP" || solver_type == "CLDDP") { + return "CLDDP"; + } + if (solver_type == "LogDDP" || solver_type == "LOGDDP") { + return "LogDDP"; + } + if (solver_type == "IPDDP") { + return "IPDDP"; + } + if (solver_type == "MSIPDDP") { + return "MSIPDDP"; + } + if (solver_type == "ALDDP") { + return "ALDDP"; + } + + return solver_type; +} } // namespace CDDPSolution CDDP::solve(SolverType solver_type) { @@ -302,25 +322,30 @@ CDDP::createSolver(const std::string &solver_type) { CDDPSolution CDDP::solve(const std::string &solver_type) { // This is where strategy selection and invocation will happen. + const std::string canonical_solver_type = canonicalizeSolverType(solver_type); initializeProblemIfNecessary(); // Ensure X_, U_ are sized etc. - // Strategy selection and instantiation - solver_ = createSolver(solver_type); + // Preserve solver state across repeated solves with the same algorithm so + // warm-start options can reuse stored gains. + if (!solver_ || solver_->getSolverName() != canonical_solver_type) { + solver_ = createSolver(canonical_solver_type); + } if (!solver_) { // Solver not found - return error solution CDDPSolution solution; - solution.solver_name = solver_type; + solution.solver_name = canonical_solver_type; solution.status_message = - "UnknownSolver - No solver registered for '" + solver_type + "'"; + "UnknownSolver - No solver registered for '" + canonical_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 + std::cout << "Solver type '" << canonical_solver_type << "' not found. Available solvers: "; auto available = getRegisteredSolvers(); for (const auto &name : available) { diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index 04fa0b2f..6d31a07c 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -16,28 +16,22 @@ limitations under the License. #include "cddp_core/logddp_solver.hpp" #include "cddp_core/cddp_core.hpp" -#include #include -#include -#include #include #include +#include namespace cddp { LogDDPSolver::LogDDPSolver() - : mu_(1e-1), relaxation_delta_(1e-5), constraint_violation_(1e+7), - ms_segment_length_(5) {} + : mu_(1e-1), relaxation_delta_(1e-5), constraint_violation_(0.0) {} void LogDDPSolver::initialize(CDDP &context) { const CDDPOptions &options = context.getOptions(); + const int horizon = context.getHorizon(); + const int control_dim = context.getControlDim(); + const int state_dim = context.getStateDim(); - int horizon = context.getHorizon(); - int control_dim = context.getControlDim(); - int state_dim = context.getStateDim(); - - // Check if reference_state in objective and reference_state in context are - // the same if ((context.getReferenceState() - context.getObjective().getReferenceState()) .norm() > 1e-6) { std::cerr << "LogDDP: Initial state and goal state in the objective " @@ -47,19 +41,48 @@ void LogDDPSolver::initialize(CDDP &context) { "Initial state and goal state in the objective function do not match"); } - // For warm starts, verify that existing state is valid + if (options.log_barrier.segment_length != horizon) { + throw std::runtime_error( + "LogDDP: segment_length must equal horizon in single-shooting mode"); + } + if (options.log_barrier.rollout_type != "nonlinear") { + throw std::runtime_error( + "LogDDP: rollout_type must be 'nonlinear' in single-shooting mode"); + } + + auto initialize_line_search = [&]() { + context.alphas_.clear(); + double alpha = options.line_search.initial_step_size; + for (int i = 0; i < options.line_search.max_iterations; ++i) { + context.alphas_.push_back(alpha); + alpha *= options.line_search.step_reduction_factor; + } + context.alpha_pr_ = options.line_search.initial_step_size; + dV_ = Eigen::Vector2d::Zero(); + }; + + auto initialize_barrier = [&]() { + mu_ = options.log_barrier.barrier.mu_initial; + relaxation_delta_ = options.log_barrier.relaxed_log_barrier_delta; + if (!relaxed_log_barrier_) { + relaxed_log_barrier_ = + std::make_unique(mu_, relaxation_delta_); + } else { + relaxed_log_barrier_->setBarrierCoeff(mu_); + relaxed_log_barrier_->setRelaxationDelta(relaxation_delta_); + } + }; + if (options.warm_start) { bool valid_warm_start = (k_u_.size() == static_cast(horizon) && K_u_.size() == static_cast(horizon) && - F_.size() == static_cast(horizon) && - context.X_.size() == static_cast(horizon + 1) && context.U_.size() == static_cast(horizon)); if (valid_warm_start && !k_u_.empty()) { 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 || F_[t].size() != state_dim) { + K_u_[t].cols() != state_dim || context.U_[t].size() != control_dim) { valid_warm_start = false; break; } @@ -73,139 +96,39 @@ void LogDDPSolver::initialize(CDDP &context) { std::cout << "LogDDP: Using warm start with existing control gains" << std::endl; } - - // Reset barrier parameters for warm start - mu_ = options.log_barrier.barrier.mu_initial; - relaxation_delta_ = options.log_barrier.relaxed_log_barrier_delta; - if (!relaxed_log_barrier_) { - relaxed_log_barrier_ = - std::make_unique(mu_, relaxation_delta_); - } else { - relaxed_log_barrier_->setBarrierCoeff(mu_); - relaxed_log_barrier_->setRelaxationDelta(relaxation_delta_); - } - - // Initialize regularization context.regularization_ = options.regularization.initial_value; - - // Initialize line search parameters - context.alphas_.clear(); - double alpha = options.line_search.initial_step_size; - for (int i = 0; i < options.line_search.max_iterations; ++i) { - context.alphas_.push_back(alpha); - alpha *= options.line_search.step_reduction_factor; - } - context.alpha_pr_ = options.line_search.initial_step_size; - dV_ = Eigen::Vector2d::Zero(); - - constraint_violation_ = std::numeric_limits::infinity(); - ms_segment_length_ = options.log_barrier.segment_length; - - // Evaluate current trajectory and reset filter + initialize_line_search(); + initialize_barrier(); evaluateTrajectory(context); resetFilter(context); return; - } else if (options.verbose) { - std::cout << "LogDDP: Warning - warm start requested but no valid solver " - "state found. " - << "Falling back to cold start initialization." << std::endl; - } - } - - // Cold start: Initialize trajectories with interpolation between initial and - // reference states - if (context.X_.size() != static_cast(horizon + 1) || - context.U_.size() != static_cast(horizon)) { - context.X_.resize(horizon + 1); - context.U_.resize(horizon); - - // Create X_ initial guess using initial_state and reference_state by - // interpolating between them - for (int t = 0; t <= horizon; ++t) { - context.X_[t] = - context.getInitialState() + - t * (context.getReferenceState() - context.getInitialState()) / - horizon; } - for (int t = 0; t < horizon; ++t) { - context.U_[t] = Eigen::VectorXd::Zero(control_dim); + if (options.verbose) { + std::cout << "LogDDP: Warning - warm start requested but no valid solver " + "state found. Falling back to cold start initialization." + << std::endl; } } - // Resize dynamics storage - F_.resize(horizon); - for (int t = 0; t < horizon; ++t) { - F_[t] = Eigen::VectorXd::Zero(state_dim); - } - - // Use base class helper for gains initializeGains(horizon, control_dim, state_dim); - G_.clear(); - - const auto &constraint_set = context.getConstraintSet(); - for (const auto &constraint_pair : constraint_set) { - const std::string &constraint_name = constraint_pair.first; - int dual_dim = constraint_pair.second->getDualDim(); - - G_[constraint_name].resize(horizon); - for (int t = 0; t < horizon; ++t) { - G_[constraint_name][t] = Eigen::VectorXd::Zero(dual_dim); + context.U_.resize(horizon); + for (int t = 0; t < horizon; ++t) { + if (context.U_[t].size() != control_dim) { + context.U_[t] = Eigen::VectorXd::Zero(control_dim); } } - // Initialize cost using objective evaluation - context.cost_ = context.getObjective().evaluate(context.X_, context.U_); - - // Initialize line search parameters - context.alphas_.clear(); - double alpha = options.line_search.initial_step_size; - for (int i = 0; i < options.line_search.max_iterations; ++i) { - context.alphas_.push_back(alpha); - alpha *= options.line_search.step_reduction_factor; - } - context.alpha_pr_ = options.line_search.initial_step_size; - dV_ = Eigen::Vector2d::Zero(); - - // Initialize regularization context.regularization_ = options.regularization.initial_value; - - constraint_violation_ = std::numeric_limits::infinity(); - - ms_segment_length_ = options.log_barrier.segment_length; - - // Check if ms_segment_length_ is valid - if (ms_segment_length_ < 0) { - std::cerr << "LogDDP: ms_segment_length_ must be non-negative" << std::endl; - throw std::runtime_error("LogDDP: ms_segment_length_ must be non-negative"); - } - - const std::string &rollout_type = options.log_barrier.rollout_type; - if (rollout_type != "linear" && rollout_type != "nonlinear" && - rollout_type != "hybrid") { - std::cerr << "LogDDP: Invalid ms_rollout_type: " << rollout_type - << std::endl; - throw std::runtime_error("LogDDP: Invalid ms_rollout_type"); - } - - // Initialize log barrier object - mu_ = options.log_barrier.barrier.mu_initial; - relaxation_delta_ = options.log_barrier.relaxed_log_barrier_delta; - if (!relaxed_log_barrier_) { - relaxed_log_barrier_ = - std::make_unique(mu_, relaxation_delta_); - } - - // Evaluate initial trajectory + initialize_line_search(); + initialize_barrier(); evaluateTrajectory(context); resetFilter(context); } std::string LogDDPSolver::getSolverName() const { return "LogDDP"; } -// === Hook implementations === - void LogDDPSolver::preIterationSetup(CDDP &context) { evaluateTrajectory(context); resetFilter(context); @@ -213,22 +136,15 @@ void LogDDPSolver::preIterationSetup(CDDP &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 + return true; } 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); - - // Also update LogDDP-specific state - if (result.dynamics_trajectory) { - F_ = *result.dynamics_trajectory; - } constraint_violation_ = result.constraint_violation; + context.inf_pr_ = result.constraint_violation; } bool LogDDPSolver::checkConvergence(CDDP &context, double dJ, double dL, @@ -264,23 +180,20 @@ 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); } void LogDDPSolver::recordIterationHistory(const CDDP &context) { - // Call base to record common metrics CDDPSolverBase::recordIterationHistory(context); - - // Add LogDDP-specific metrics history_.barrier_mu.push_back(mu_); } @@ -302,258 +215,165 @@ void LogDDPSolver::printIteration(int iter, const CDDP &context) const { 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; + << 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; - // Rollout dynamics and calculate cost + context.X_.resize(horizon + 1); + context.X_[0] = context.getInitialState(); + context.cost_ = 0.0; + for (int t = 0; t < horizon; ++t) { const Eigen::VectorXd &x_t = context.X_[t]; const Eigen::VectorXd &u_t = context.U_[t]; - // Compute stage cost using the guessed state/control - cost += context.getObjective().running_cost(x_t, u_t, t); - - // Compute dynamics - F_[t] = context.getSystem().getDiscreteDynamics(x_t, u_t, - t * context.getTimestep()); + context.cost_ += context.getObjective().running_cost(x_t, u_t, t); + context.X_[t + 1] = context.getSystem().getDiscreteDynamics( + x_t, u_t, t * context.getTimestep()); } - // Add terminal cost based on the final guessed state - cost += context.getObjective().terminal_cost(context.X_.back()); - - // Store the initial total cost - context.cost_ = cost; + context.cost_ += context.getObjective().terminal_cost(context.X_.back()); } void LogDDPSolver::resetFilter(CDDP &context) { - // Evaluate log-barrier cost (includes path constraints) - context.merit_function_ = context.cost_; - constraint_violation_ = 0.0; - double defect_violation = 0.0; + context.merit_function_ = + computeBarrierMerit(context, context.X_, context.U_, &constraint_violation_); + context.inf_pr_ = constraint_violation_; +} - const auto &constraint_set = context.getConstraintSet(); +void LogDDPSolver::augmentRunningCostDerivatives( + const CDDP &context, int t, const Eigen::VectorXd &x, + const Eigen::VectorXd &u, Eigen::VectorXd &l_x, Eigen::VectorXd &l_u, + Eigen::MatrixXd &l_xx, Eigen::MatrixXd &l_uu, + Eigen::MatrixXd &l_ux) const { + for (const auto &[name, constraint] : context.getConstraintSet()) { + auto [L_x_relaxed, L_u_relaxed] = + relaxed_log_barrier_->getGradients(*constraint, x, u, t); + l_x += L_x_relaxed; + l_u += L_u_relaxed; + + auto [L_xx_relaxed, L_uu_relaxed, L_ux_relaxed] = + relaxed_log_barrier_->getHessians(*constraint, x, u, t); + l_xx += L_xx_relaxed; + l_uu += L_uu_relaxed; + l_ux += L_ux_relaxed; + } +} + +void LogDDPSolver::augmentTerminalCostDerivatives(const CDDP &context, + const Eigen::VectorXd &x_N, + Eigen::VectorXd &V_x, + Eigen::MatrixXd &V_xx) const { + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + const int horizon = context.getHorizon(); + + for (const auto &[name, constraint] : context.getTerminalConstraintSet()) { + auto [L_x_relaxed, L_u_relaxed] = + relaxed_log_barrier_->getGradients(*constraint, x_N, dummy_u, horizon); + (void)L_u_relaxed; + V_x += L_x_relaxed; + + auto [L_xx_relaxed, L_uu_relaxed, L_ux_relaxed] = + relaxed_log_barrier_->getHessians(*constraint, x_N, dummy_u, horizon); + (void)L_uu_relaxed; + (void)L_ux_relaxed; + V_xx += L_xx_relaxed; + } +} + +double LogDDPSolver::computeBarrierMerit( + const CDDP &context, const std::vector &X, + const std::vector &U, double *max_constraint_violation) const { + double merit = 0.0; + double max_violation = 0.0; - // Calculate path constraint terms and violation for (int t = 0; t < context.getHorizon(); ++t) { - for (const auto &constraint_pair : constraint_set) { - const std::string &constraint_name = constraint_pair.first; - Eigen::VectorXd g_t = - constraint_pair.second->evaluate(context.X_[t], context.U_[t]) - - constraint_pair.second->getUpperBound(); - G_[constraint_name][t] = g_t; - context.merit_function_ += relaxed_log_barrier_->evaluate( - *constraint_pair.second, context.X_[t], context.U_[t]); - - for (int i = 0; i < g_t.size(); ++i) { - if (g_t(i) > 0.0) { - constraint_violation_ += g_t(i); - } - } + merit += context.getObjective().running_cost(X[t], U[t], t); + + for (const auto &[name, constraint] : context.getConstraintSet()) { + merit += relaxed_log_barrier_->evaluate(*constraint, X[t], U[t], t); + max_violation = std::max( + max_violation, constraint->computeViolation(X[t], U[t], t)); } + } + + merit += context.getObjective().terminal_cost(X.back()); - // Add defect violation penalty - Eigen::VectorXd d = F_[t] - context.X_[t + 1]; - defect_violation += d.lpNorm<1>(); + Eigen::VectorXd dummy_u = Eigen::VectorXd::Zero(context.getControlDim()); + for (const auto &[name, constraint] : context.getTerminalConstraintSet()) { + merit += + relaxed_log_barrier_->evaluate(*constraint, X.back(), dummy_u, context.getHorizon()); + max_violation = std::max(max_violation, constraint->computeViolation( + X.back(), dummy_u, + context.getHorizon())); + } + + if (max_constraint_violation != nullptr) { + *max_constraint_violation = max_violation; } - constraint_violation_ += defect_violation; - context.inf_pr_ = constraint_violation_; -} -// === Backward pass === + return merit; +} 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 - // 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); - F_u_.resize(horizon); - F_xx_.resize(horizon); - F_uu_.resize(horizon); - F_ux_.resize(horizon); - - if (!use_parallel) { - for (int t = 0; t < horizon; ++t) { - const Eigen::VectorXd &x = context.X_[t]; - const Eigen::VectorXd &u = context.U_[t]; - - const auto [Fx, Fu] = - context.getSystem().getJacobians(x, u, t * timestep); - F_x_[t] = Fx; - F_u_[t] = Fu; - - if (!options.use_ilqr) { - const auto hessians = - context.getSystem().getHessians(x, u, t * timestep); - F_xx_[t] = std::get<0>(hessians); - F_uu_[t] = std::get<1>(hessians); - F_ux_[t] = std::get<2>(hessians); - } else { - F_xx_[t] = std::vector(); - F_uu_[t] = std::vector(); - F_ux_[t] = std::vector(); - } - } - } else { - 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); - - std::vector> futures; - futures.reserve(num_threads); - - for (int thread_id = 0; thread_id < num_threads; ++thread_id) { - int start_t = thread_id * chunk_size; - int end_t = (thread_id == num_threads - 1) ? horizon - : (thread_id + 1) * chunk_size; - - if (start_t >= horizon) - break; - - futures.push_back( - std::async(std::launch::async, [this, &context, &options, start_t, - end_t, timestep]() { - for (int t = start_t; t < end_t; ++t) { - const Eigen::VectorXd &x = context.X_[t]; - const Eigen::VectorXd &u = context.U_[t]; - - const auto [Fx, Fu] = - context.getSystem().getJacobians(x, u, t * timestep); - F_x_[t] = Fx; - F_u_[t] = Fu; - - if (!options.use_ilqr) { - const auto hessians = - context.getSystem().getHessians(x, u, t * timestep); - F_xx_[t] = std::get<0>(hessians); - F_uu_[t] = std::get<1>(hessians); - F_ux_[t] = std::get<2>(hessians); - } else { - F_xx_[t] = std::vector(); - F_uu_[t] = std::vector(); - F_ux_[t] = std::vector(); - } - } - })); - } - for (auto &future : futures) { - try { - if (future.valid()) { - future.get(); - } - } catch (const std::exception &e) { - if (options.verbose) { - std::cerr - << "LogDDP: Dynamics derivatives computation thread failed: " - << e.what() << std::endl; - } - throw; - } - } - } - - if (options.debug) { - std::cout << "[LogDDP] Pre-computed dynamics derivatives for " << horizon - << " time steps using " - << (use_parallel ? "parallel" : "sequential") << " computation" - << std::endl; - } + precomputeDynamicsDerivatives(context, 20); - // Terminal cost and derivatives (V_x, V_xx at t=N) 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 + augmentTerminalCostDerivatives(context, context.X_.back(), V_x, V_xx); + V_xx = 0.5 * (V_xx + V_xx.transpose()); dV_ = Eigen::Vector2d::Zero(); double Qu_err = 0.0; + double norm_Vx = V_x.lpNorm<1>(); - // 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]; - const Eigen::VectorXd &f = F_[t]; - const Eigen::VectorXd &d = f - context.X_[t + 1]; // Defect + const Eigen::MatrixXd &A = F_x_[t]; + const Eigen::MatrixXd &B = F_u_[t]; - // 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 = - timestep * Fx + Eigen::MatrixXd::Identity(state_dim, state_dim); - const Eigen::MatrixXd &B = timestep * Fu; - - // Cost derivatives at (x_t, u_t) auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = context.getObjective().getRunningCostHessians(x, u, t); + augmentRunningCostDerivatives(context, t, x, u, l_x, l_u, l_xx, l_uu, l_ux); - Eigen::VectorXd Q_x = l_x + A.transpose() * (V_x + V_xx * d); - Eigen::VectorXd Q_u = l_u + B.transpose() * (V_x + V_xx * d); + Eigen::VectorXd Q_x = l_x + A.transpose() * V_x; + Eigen::VectorXd Q_u = l_u + 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) { const auto &Fxx = F_xx_[t]; const auto &Fuu = F_uu_[t]; const auto &Fux = F_ux_[t]; - for (int i = 0; i < state_dim; ++i) { - Q_xx += timestep * V_x(i) * Fxx[i]; - Q_ux += timestep * V_x(i) * Fux[i]; - Q_uu += timestep * V_x(i) * Fuu[i]; + Q_xx += V_x(i) * Fxx[i]; + Q_ux += V_x(i) * Fux[i]; + Q_uu += V_x(i) * Fuu[i]; } } - // Apply Log-barrier cost gradients and Hessians - for (const auto &constraint_pair : constraint_set) { - auto [L_x_relaxed, L_u_relaxed] = - relaxed_log_barrier_->getGradients(*constraint_pair.second, x, u); - Q_x += L_x_relaxed; - Q_u += L_u_relaxed; - - auto [L_xx_relaxed, L_uu_relaxed, L_ux_relaxed] = - relaxed_log_barrier_->getHessians(*constraint_pair.second, x, u); - Q_xx += L_xx_relaxed; - Q_uu += L_uu_relaxed; - Q_ux += L_ux_relaxed; - } - - // Regularization - Eigen::MatrixXd Q_uu_reg = Q_uu; + Eigen::MatrixXd Q_uu_reg = 0.5 * (Q_uu + Q_uu.transpose()); Q_uu_reg.diagonal().array() += context.regularization_; - Q_uu_reg = 0.5 * (Q_uu_reg + Q_uu_reg.transpose()); // symmetrize Eigen::LDLT ldlt(Q_uu_reg); if (ldlt.info() != Eigen::Success) { @@ -563,45 +383,39 @@ bool LogDDPSolver::backwardPass(CDDP &context) { return false; } - Eigen::MatrixXd bigRHS(control_dim, 1 + state_dim); - bigRHS.col(0) = Q_u; - Eigen::MatrixXd M = Q_ux; - for (int col = 0; col < state_dim; col++) { - bigRHS.col(col + 1) = M.col(col); - } - - Eigen::MatrixXd kK = -ldlt.solve(bigRHS); + Eigen::MatrixXd rhs(control_dim, 1 + state_dim); + rhs.col(0) = Q_u; + rhs.rightCols(state_dim) = Q_ux; - // parse out feedforward (ku) and feedback (Ku) - Eigen::VectorXd k_u = kK.col(0); - Eigen::MatrixXd K_u(control_dim, state_dim); - for (int col = 0; col < state_dim; col++) { - K_u.col(col) = kK.col(col + 1); - } + Eigen::MatrixXd gains = -ldlt.solve(rhs); + const Eigen::VectorXd k_u = gains.col(0); + const Eigen::MatrixXd K_u = gains.rightCols(state_dim); - // Save gains k_u_[t] = k_u; K_u_[t] = K_u; - // Compute value function approximation - Eigen::Vector2d dV_step; - dV_step << Q_u.dot(k_u), 0.5 * k_u.dot(Q_uu * k_u); - dV_ = dV_ + dV_step; + dV_(0) += Q_u.dot(k_u); + dV_(1) += 0.5 * k_u.dot(Q_uu * k_u); + V_x = Q_x + K_u.transpose() * Q_uu * k_u + Q_ux.transpose() * k_u + K_u.transpose() * Q_u; V_xx = Q_xx + K_u.transpose() * Q_uu * K_u + Q_ux.transpose() * K_u + K_u.transpose() * Q_ux; - V_xx = 0.5 * (V_xx + V_xx.transpose()); // Symmetrize Hessian + V_xx = 0.5 * (V_xx + V_xx.transpose()); - // Compute optimality gap (Inf-norm) for convergence check Qu_err = std::max(Qu_err, Q_u.lpNorm()); + norm_Vx += V_x.lpNorm<1>(); } - context.inf_du_ = Qu_err; + + 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_err / scaling_factor; if (options.debug) { std::cout << "[LogDDP Backward Pass]\n" << " Qu_err: " << std::scientific << std::setprecision(4) - << Qu_err << "\n" + << context.inf_du_ << "\n" << " rf_err: " << std::scientific << std::setprecision(4) << constraint_violation_ << "\n" << " dV: " << std::scientific << std::setprecision(4) @@ -611,11 +425,8 @@ bool LogDDPSolver::backwardPass(CDDP &context) { return true; } -// === Forward pass === - ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { const CDDPOptions &options = context.getOptions(); - const auto &constraint_set = context.getConstraintSet(); ForwardPassResult result; result.success = false; @@ -623,120 +434,50 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { result.merit_function = std::numeric_limits::infinity(); result.alpha_pr = alpha; - const int horizon = context.getHorizon(); - const int state_dim = context.getStateDim(); - - // Initialize trajectories result.state_trajectory = context.X_; result.control_trajectory = context.U_; result.state_trajectory[0] = context.getInitialState(); - std::vector F_new = F_; - double cost_new = 0.0; - double merit_function_new = 0.0; - double rp_err = 0.0; - double rf_err = 0.0; - - // Rollout loop with multi-shooting logic from original - for (int t = 0; t < horizon; ++t) { - const Eigen::VectorXd delta_x_t = - result.state_trajectory[t] - context.X_[t]; - // Update control + for (int t = 0; t < context.getHorizon(); ++t) { + const Eigen::VectorXd delta_x_t = result.state_trajectory[t] - context.X_[t]; result.control_trajectory[t] = context.U_[t] + alpha * k_u_[t] + K_u_[t] * delta_x_t; - // --- Rollout Logic from original --- - Eigen::VectorXd dynamics_eval_for_F_new_t; - - // Determine if the *next* step (t+1) starts a new segment boundary - bool is_segment_boundary = (ms_segment_length_ > 0) && - ((t + 1) % ms_segment_length_ == 0) && - (t + 1 < horizon); - bool apply_gap_closing_strategy = is_segment_boundary; - - if (apply_gap_closing_strategy) { - if (options.log_barrier.rollout_type == "nonlinear") { - F_new[t] = context.getSystem().getDiscreteDynamics( - result.state_trajectory[t], result.control_trajectory[t], - t * context.getTimestep()); - result.state_trajectory[t + 1] = context.X_[t + 1] + - (F_new[t] - F_[t]) + - alpha * (F_[t] - context.X_[t + 1]); - } else if (options.log_barrier.rollout_type == "hybrid") { - F_new[t] = context.getSystem().getDiscreteDynamics( - result.state_trajectory[t], result.control_trajectory[t], - t * context.getTimestep()); - const auto [Fx, Fu] = context.getSystem().getJacobians( - context.X_[t], context.U_[t], t * context.getTimestep()); - const Eigen::MatrixXd A = - Eigen::MatrixXd::Identity(state_dim, state_dim) + - context.getTimestep() * Fx; - const Eigen::MatrixXd B = context.getTimestep() * Fu; - result.state_trajectory[t + 1] = - context.X_[t + 1] + (A + B * K_u_[t]) * delta_x_t + - alpha * (B * k_u_[t] + F_[t] - context.X_[t + 1]); - } - } else { - result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( - result.state_trajectory[t], result.control_trajectory[t], - t * context.getTimestep()); - F_new[t] = result.state_trajectory[t + 1]; + if (!result.control_trajectory[t].allFinite()) { + return result; } - // Robustness check - if (!result.state_trajectory[t + 1].allFinite() || - !result.control_trajectory[t].allFinite()) { + cost_new += context.getObjective().running_cost(result.state_trajectory[t], + result.control_trajectory[t], t); + result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics( + result.state_trajectory[t], result.control_trajectory[t], + t * context.getTimestep()); + + if (!result.state_trajectory[t + 1].allFinite()) { if (options.debug) { std::cerr << "[LogDDP Forward Pass] NaN/Inf detected during rollout at t=" << t << " for alpha=" << alpha << std::endl; } - result.success = false; return result; } } - // Cost computation and filter line-search from original - for (int t = 0; t < horizon; ++t) { - cost_new += context.getObjective().running_cost( - result.state_trajectory[t], result.control_trajectory[t], t); - - for (const auto &constraint_pair : constraint_set) { - const std::string &constraint_name = constraint_pair.first; - Eigen::VectorXd g_t = - constraint_pair.second->evaluate(result.state_trajectory[t], - result.control_trajectory[t]) - - constraint_pair.second->getUpperBound(); - merit_function_new += relaxed_log_barrier_->evaluate( - *constraint_pair.second, result.state_trajectory[t], - result.control_trajectory[t]); - - for (int i = 0; i < g_t.size(); ++i) { - if (g_t(i) > 0.0) { - rp_err += g_t(i); - } - } - } - - Eigen::VectorXd d = F_new[t] - result.state_trajectory[t + 1]; - rf_err += d.lpNorm<1>(); - } + cost_new += context.getObjective().terminal_cost(result.state_trajectory.back()); - cost_new += - context.getObjective().terminal_cost(result.state_trajectory.back()); - merit_function_new += cost_new; - - // Filter-based acceptance using original logic with new options structure - double constraint_violation_old = constraint_violation_; - double constraint_violation_new = rf_err + rp_err; - double merit_function_old = context.merit_function_; - bool filter_acceptance = false; - double expected_improvement = alpha * dV_(0); + double constraint_violation_new = 0.0; + double merit_function_new = computeBarrierMerit( + context, result.state_trajectory, result.control_trajectory, + &constraint_violation_new); + const double constraint_violation_old = constraint_violation_; + const double merit_function_old = context.merit_function_; + const double expected_improvement = alpha * dV_(0); const auto &filter_opts = options.filter; + bool filter_acceptance = false; if (constraint_violation_new > filter_opts.max_violation_threshold) { if (constraint_violation_new < (1.0 - filter_opts.violation_acceptance_threshold) * @@ -745,9 +486,10 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { } } else if (std::max(constraint_violation_new, constraint_violation_old) < filter_opts.min_violation_for_armijo_check && - expected_improvement < 0) { - if (merit_function_new < merit_function_old + filter_opts.armijo_constant * - expected_improvement) { + expected_improvement < 0.0) { + if (merit_function_new < merit_function_old + + filter_opts.armijo_constant * + expected_improvement) { filter_acceptance = true; } } else { @@ -766,7 +508,6 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) { result.cost = cost_new; result.merit_function = merit_function_new; result.constraint_violation = constraint_violation_new; - result.dynamics_trajectory = F_new; } return result; diff --git a/tests/cddp_core/test_alddp_solver.cpp b/tests/cddp_core/test_alddp_solver.cpp index b5a413a7..ee8bf152 100644 --- a/tests/cddp_core/test_alddp_solver.cpp +++ b/tests/cddp_core/test_alddp_solver.cpp @@ -597,3 +597,64 @@ TEST(ALDDPTest, ConstraintFreeMatchesCLDDP) { EXPECT_LT(ratio, 2.0); } } + +TEST(ALDDPTest, WarmStartRefreshesSlackControlsForReseededTrajectory) { + const int state_dim = 4; + const int control_dim = 1; + const int horizon = 100; + const double timestep = 0.02; + + auto system = std::make_unique(timestep, "rk4"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = 0.01 * Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = 100.0 * Eigen::MatrixXd::Identity(state_dim, state_dim); + + Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim); + goal_state << 0.0, M_PI, 0.0, 0.0; + + std::vector empty_ref; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_ref, timestep); + + Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(state_dim); + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + Eigen::VectorXd u_lb(control_dim), u_ub(control_dim); + u_lb << -20.0; + u_ub << 20.0; + solver.addPathConstraint("ControlConstraint", + std::make_unique(u_lb, u_ub)); + + auto options = makeALDDPOptions(); + options.verbose = false; + options.print_solver_header = false; + solver.setOptions(options); + solver.setInitialTrajectory( + std::vector(horizon + 1, initial_state), + std::vector(horizon, Eigen::VectorXd::Zero(control_dim))); + + auto first_solution = solver.solve(cddp::SolverType::ALDDP); + ASSERT_FALSE(first_solution.state_trajectory.empty()); + + std::vector X_infeasible(horizon + 1); + for (int t = 0; t <= horizon; ++t) { + double alpha = static_cast(t) / horizon; + X_infeasible[t] = (1.0 - alpha) * initial_state + alpha * goal_state; + } + std::vector U_seed(horizon, Eigen::VectorXd::Zero(control_dim)); + + options.warm_start = true; + options.max_iterations = 100; + solver.setOptions(options); + solver.setInitialTrajectory(X_infeasible, U_seed); + + auto reseeded_solution = solver.solve(cddp::SolverType::ALDDP); + + EXPECT_GT(reseeded_solution.iterations_completed, 0); + EXPECT_FALSE(reseeded_solution.state_trajectory.empty()); + EXPECT_TRUE(std::isfinite(reseeded_solution.final_objective)); +} diff --git a/tests/cddp_core/test_logddp_solver.cpp b/tests/cddp_core/test_logddp_solver.cpp index a028d342..96ddbbb3 100644 --- a/tests/cddp_core/test_logddp_solver.cpp +++ b/tests/cddp_core/test_logddp_solver.cpp @@ -99,6 +99,8 @@ TEST(LogDDPTest, SolvePendulum) options.verbose = true; options.debug = false; options.regularization.initial_value = 1e-6; + options.log_barrier.segment_length = horizon; + options.log_barrier.rollout_type = "nonlinear"; options.return_iteration_info = true; // Get detailed iteration history // Set options @@ -229,6 +231,94 @@ TEST(LogDDPTest, SolvePendulum) EXPECT_LE(warm_iterations, iterations_completed + 5) << "Warm start should not take significantly more iterations"; } +TEST(LogDDPTest, RecomputesStateTrajectoryFromSeededControlsOnColdStart) { + int state_dim = 2; + int control_dim = 1; + int horizon = 5; + double timestep = 0.05; + + auto system = std::make_unique(timestep, 1.0, 1.0, 0.0, "euler"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::VectorXd goal_state(state_dim); + goal_state << 0.0, 0.0; + + std::vector empty_reference_states; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_reference_states, timestep); + + Eigen::VectorXd initial_state(state_dim); + initial_state << M_PI, 0.0; + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + std::vector X_seed(horizon + 1, Eigen::VectorXd::Zero(state_dim)); + std::vector U_seed(horizon, Eigen::VectorXd::Zero(control_dim)); + X_seed[0] = initial_state; + for (int t = 1; t <= horizon; ++t) { + X_seed[t] << initial_state(0) - 0.1 * t, 0.2 * t; + } + solver.setInitialTrajectory(X_seed, U_seed); + + cddp::CDDPOptions options; + options.max_iterations = 0; + options.verbose = false; + options.debug = false; + options.log_barrier.segment_length = horizon; + options.log_barrier.rollout_type = "nonlinear"; + solver.setOptions(options); + + cddp::CDDPSolution solution = solver.solve("LogDDP"); + Eigen::VectorXd expected_next_state = + solver.getSystem().getDiscreteDynamics(initial_state, U_seed[0], 0.0); + + ASSERT_EQ(solution.state_trajectory.size(), X_seed.size()); + EXPECT_NEAR(solution.state_trajectory[1](0), expected_next_state(0), 1e-12); + EXPECT_NEAR(solution.state_trajectory[1](1), expected_next_state(1), 1e-12); + EXPECT_GT((solution.state_trajectory[1] - X_seed[1]).norm(), 1e-6); + EXPECT_EQ(solution.status_message, "MaxIterationsReached"); +} + +TEST(LogDDPTest, RejectsSegmentedRolloutOptions) { + int state_dim = 2; + int control_dim = 1; + int horizon = 5; + double timestep = 0.05; + + auto system = std::make_unique(timestep, 1.0, 1.0, 0.0, "euler"); + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim); + Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim); + Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim); + Eigen::VectorXd goal_state(state_dim); + goal_state << 0.0, 0.0; + + std::vector empty_reference_states; + auto objective = std::make_unique( + Q, R, Qf, goal_state, empty_reference_states, timestep); + + Eigen::VectorXd initial_state(state_dim); + initial_state << M_PI, 0.0; + + cddp::CDDP solver(initial_state, goal_state, horizon, timestep); + solver.setDynamicalSystem(std::move(system)); + solver.setObjective(std::move(objective)); + + cddp::CDDPOptions options; + options.max_iterations = 0; + options.verbose = false; + options.debug = false; + options.log_barrier.segment_length = horizon - 1; + options.log_barrier.rollout_type = "hybrid"; + solver.setOptions(options); + + EXPECT_THROW(solver.solve("LogDDP"), std::runtime_error); +} + TEST(LogDDPTest, SolveUnicycle) { // Problem parameters int state_dim = 3; @@ -267,6 +357,8 @@ TEST(LogDDPTest, SolveUnicycle) { options.num_threads = 10; options.verbose = true; options.debug = false; + options.log_barrier.segment_length = horizon; + options.log_barrier.rollout_type = "nonlinear"; // Create CDDP solver cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep); @@ -495,26 +587,18 @@ TEST(LogDDPTest, SolveCar) warm_options.warm_start = true; warm_options.max_iterations = 100; // Allow sufficient iterations for warm start convergence warm_options.verbose = false; // Less verbose for warm start test + warm_options.tolerance = 1e-3; + warm_options.acceptable_tolerance = 1e-3; + warm_options.regularization.initial_value = 1e-4; - // Create a new solver for warm start test - auto car_system_warmstart = std::make_unique(timestep, wheelbase, integration_type); - - // Create new objective - auto objective_warmstart = std::make_unique(goal_state, timestep); - - cddp::CDDP warm_solver(initial_state, goal_state, horizon, timestep); - warm_solver.setDynamicalSystem(std::move(car_system_warmstart)); - warm_solver.setObjective(std::move(objective_warmstart)); - warm_solver.addPathConstraint("ControlConstraint", - std::make_unique(control_lower_bound, control_upper_bound)); - warm_solver.setOptions(warm_options); - - // Use previous solution as warm start - warm_solver.setInitialTrajectory(X_sol, U_sol); + // Reuse the same solver instance so LogDDP can preserve its warm-start + // gains, matching the documented contract of the option. + cddp_solver.setOptions(warm_options); + cddp_solver.setInitialTrajectory(X_sol, U_sol); // Solve with warm start auto start_time = std::chrono::high_resolution_clock::now(); - cddp::CDDPSolution warm_solution = warm_solver.solve("LogDDP"); + cddp::CDDPSolution warm_solution = cddp_solver.solve("LogDDP"); auto end_time = std::chrono::high_resolution_clock::now(); auto warm_duration = std::chrono::duration_cast(end_time - start_time);