From c91e5182fa638b0ff95270cedd0ebe3c52b40c3f Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Sun, 29 Mar 2026 19:24:31 -0400 Subject: [PATCH 01/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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 2bad33be5a96f2a95cc2eeff0e7500623f3e299a Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Wed, 1 Apr 2026 14:54:49 -0400 Subject: [PATCH 13/17] Fix Python binding validation and solver alias handling Reject abstract base instances before the solver owns them and keep native callbacks on the no-GIL fast path. Add regression coverage for solver aliases, parallel native callbacks, and the isolated import error path. --- python/src/bind_solver.cpp | 360 ++++++++++++++++++++--------- python/tests/test_constraints.py | 26 +++ python/tests/test_pendulum.py | 31 +++ python/tests/test_solver_errors.py | 39 +++- 4 files changed, 341 insertions(+), 115 deletions(-) diff --git a/python/src/bind_solver.cpp b/python/src/bind_solver.cpp index 65fde2b3..74b6b180 100644 --- a/python/src/bind_solver.cpp +++ b/python/src/bind_solver.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -11,27 +12,95 @@ namespace py = pybind11; namespace { -constexpr std::array kBuiltinSolverNames = { - "CLDDP", "LogDDP", "IPDDP", "MSIPDDP"}; +template +auto callWrapped(bool requires_gil, Func &&func) -> decltype(func()) { + if (requires_gil) { + py::gil_scoped_acquire gil; + return func(); + } + return func(); +} + +template +bool isExactCoreType(const py::handle &object, + const std::array &type_names) { + const py::type object_type = py::type::of(object); + const std::string module = py::str(object_type.attr("__module__")); + if (module != "pycddp._pycddp_core") { + return false; + } -bool isBuiltinSolverName(const std::string &solver_name) { - for (const char *name : kBuiltinSolverNames) { - if (solver_name == name) { + const std::string name = py::str(object_type.attr("__name__")); + for (std::string_view type_name : type_names) { + if (name == type_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(); +bool nativeDynamicsCanSkipGil(const py::handle &object) { + static constexpr std::array kNativeDynamicsTypes = { + "Pendulum", "Unicycle", "Bicycle", + "Car", "CartPole", "DubinsCar", + "Forklift", "Acrobot", "Quadrotor", + "QuadrotorRate", "Manipulator", "HCW", + "SpacecraftLinearFuel", "SpacecraftNonlinear", + "DreyfusRocket", "SpacecraftLanding2D", + "SpacecraftROE", "SpacecraftTwobody", + "LTISystem", "Usv3Dof", "EulerAttitude", + "QuaternionAttitude", "MrpAttitude", + }; + return isExactCoreType(object, kNativeDynamicsTypes); +} + +bool nativeObjectiveCanSkipGil(const py::handle &object) { + static constexpr std::array kNativeObjectiveTypes = { + "QuadraticObjective", + }; + return isExactCoreType(object, kNativeObjectiveTypes); +} + +bool nativeConstraintCanSkipGil(const py::handle &object) { + static constexpr std::array kNativeConstraintTypes = { + "ControlConstraint", "StateConstraint", + "LinearConstraint", "BallConstraint", + "PoleConstraint", "SecondOrderConeConstraint", + "ThrustMagnitudeConstraint", "MaxThrustMagnitudeConstraint", + }; + return isExactCoreType(object, kNativeConstraintTypes); +} + +bool isExactConstraintBase(const py::handle &object) { + static constexpr std::array kConstraintBaseTypes = { + "Constraint", + }; + return isExactCoreType(object, kConstraintBaseTypes); +} + +bool isExactDynamicsBase(const py::handle &object) { + static constexpr std::array kDynamicsBaseTypes = { + "DynamicalSystem", + }; + return isExactCoreType(object, kDynamicsBaseTypes); +} + +bool isExactObjectiveBase(const py::handle &object) { + static constexpr std::array kObjectiveBaseTypes = { + "Objective", + }; + return isExactCoreType(object, kObjectiveBaseTypes); +} + +bool isKnownSolverName(const std::string &solver_name) { + return solver_name == "CLDDP" || solver_name == "CLCDDP" || + solver_name == "LogDDP" || solver_name == "LOGDDP" || + solver_name == "IPDDP" || solver_name == "MSIPDDP" || + cddp::CDDP::isSolverRegistered(solver_name); +} + +bool isUnknownSolverStatus(const std::string &status_message) { + return status_message.rfind("UnknownSolver", 0) == 0; } void validateInitialTrajectory(cddp::CDDP &solver, @@ -86,15 +155,17 @@ void validateInitialTrajectory(cddp::CDDP &solver, // 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 forwarded method reacquires the GIL because solver work may run on -// worker threads when parallel execution is enabled. +// and each forwarded method only reacquires the GIL when the object is truly +// Python-backed. Native extension types can stay on the fast path. class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { public: - PythonBackedDynamicalSystem(py::object owner, cddp::DynamicalSystem *wrapped) + PythonBackedDynamicalSystem(py::object owner, cddp::DynamicalSystem *wrapped, + bool requires_gil) : cddp::DynamicalSystem(wrapped->getStateDim(), wrapped->getControlDim(), wrapped->getTimestep(), wrapped->getIntegrationType()), - owner_(std::move(owner)), wrapped_(wrapped) {} + owner_(std::move(owner)), wrapped_(wrapped), + requires_gil_(requires_gil) {} ~PythonBackedDynamicalSystem() override { try { @@ -108,70 +179,81 @@ class PythonBackedDynamicalSystem : public cddp::DynamicalSystem { 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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_gil_, [&] { + return wrapped_->getCrossHessian(state, control, time); + }); } private: py::object owner_; cddp::DynamicalSystem *wrapped_; + bool requires_gil_; }; class PythonBackedObjective : public cddp::Objective { public: - PythonBackedObjective(py::object owner, cddp::Objective *wrapped) - : owner_(std::move(owner)), wrapped_(wrapped) {} + PythonBackedObjective(py::object owner, cddp::Objective *wrapped, + bool requires_gil) + : owner_(std::move(owner)), wrapped_(wrapped), + requires_gil_(requires_gil) {} ~PythonBackedObjective() override { try { @@ -184,105 +266,117 @@ class PythonBackedObjective : public cddp::Objective { double evaluate(const std::vector &states, const std::vector &controls) const override { - py::gil_scoped_acquire gil; - return wrapped_->evaluate(states, controls); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_gil_, [&] { + return wrapped_->getFinalCostHessian(final_state); + }); } Eigen::VectorXd getReferenceState() const override { - py::gil_scoped_acquire gil; - return wrapped_->getReferenceState(); + return callWrapped(requires_gil_, + [&] { return wrapped_->getReferenceState(); }); } std::vector getReferenceStates() const override { - py::gil_scoped_acquire gil; - return wrapped_->getReferenceStates(); + return callWrapped(requires_gil_, + [&] { return wrapped_->getReferenceStates(); }); } void setReferenceState(const Eigen::VectorXd &reference_state) override { - py::gil_scoped_acquire gil; - wrapped_->setReferenceState(reference_state); + callWrapped(requires_gil_, + [&] { wrapped_->setReferenceState(reference_state); }); } void setReferenceStates( const std::vector &reference_states) override { - py::gil_scoped_acquire gil; - wrapped_->setReferenceStates(reference_states); + callWrapped(requires_gil_, + [&] { wrapped_->setReferenceStates(reference_states); }); } private: py::object owner_; cddp::Objective *wrapped_; + bool requires_gil_; }; class PythonBackedConstraint : public cddp::Constraint { public: - PythonBackedConstraint(py::object owner, cddp::Constraint *wrapped) + PythonBackedConstraint(py::object owner, cddp::Constraint *wrapped, + bool requires_gil) : cddp::Constraint(wrapped->getName()), owner_(std::move(owner)), - wrapped_(wrapped) {} + wrapped_(wrapped), requires_gil_(requires_gil) {} ~PythonBackedConstraint() override { try { @@ -294,85 +388,132 @@ class PythonBackedConstraint : public cddp::Constraint { } int getDualDim() const override { - py::gil_scoped_acquire gil; - return wrapped_->getDualDim(); + return callWrapped(requires_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); + return callWrapped(requires_gil_, [&] { + return wrapped_->evaluate(state, control, index); + }); } Eigen::VectorXd getLowerBound() const override { - py::gil_scoped_acquire gil; - return wrapped_->getLowerBound(); + return callWrapped(requires_gil_, + [&] { return wrapped_->getLowerBound(); }); } Eigen::VectorXd getUpperBound() const override { - py::gil_scoped_acquire gil; - return wrapped_->getUpperBound(); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_gil_, [&] { + return wrapped_->computeViolation(state, control, index); + }); } double computeViolationFromValue(const Eigen::VectorXd &g) const override { - py::gil_scoped_acquire gil; - return wrapped_->computeViolationFromValue(g); + return callWrapped(requires_gil_, [&] { + return wrapped_->computeViolationFromValue(g); + }); } Eigen::VectorXd getCenter() const override { - py::gil_scoped_acquire gil; - return wrapped_->getCenter(); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_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); + return callWrapped(requires_gil_, [&] { + return wrapped_->getCrossHessian(state, control, index); + }); } private: py::object owner_; cddp::Constraint *wrapped_; + bool requires_gil_; }; +std::unique_ptr makeOwnedDynamicalSystem( + py::object system) { + if (isExactDynamicsBase(system)) { + throw py::type_error( + "pycddp.DynamicalSystem is an abstract base class. " + "Pass a concrete built-in model or a Python subclass that " + "implements the required methods."); + } + auto *wrapped = system.cast(); + const bool requires_gil = !nativeDynamicsCanSkipGil(system); + return std::make_unique( + std::move(system), wrapped, requires_gil); +} + +std::unique_ptr makeOwnedObjective(py::object objective) { + if (isExactObjectiveBase(objective)) { + throw py::type_error( + "pycddp.Objective is an abstract base class. " + "Pass a concrete built-in objective or a Python subclass that " + "implements the required methods."); + } + auto *wrapped = objective.cast(); + const bool requires_gil = !nativeObjectiveCanSkipGil(objective); + return std::make_unique( + std::move(objective), wrapped, requires_gil); +} + +std::unique_ptr makeOwnedConstraint(py::object constraint) { + if (isExactConstraintBase(constraint)) { + throw py::type_error( + "pycddp.Constraint is an abstract base class. " + "Pass a concrete built-in constraint or a Python subclass that " + "implements the required methods."); + } + auto *wrapped = constraint.cast(); + const bool requires_gil = !nativeConstraintCanSkipGil(constraint); + return std::make_unique( + std::move(constraint), wrapped, requires_gil); +} + } // namespace void bind_solver(py::module_ &m) { @@ -445,10 +586,8 @@ void bind_solver(py::module_ &m) { .def("set_options", &cddp::CDDP::setOptions, py::arg("options")) .def("set_dynamical_system", [](cddp::CDDP &self, py::object system) { - auto *wrapped = system.cast(); - self.setDynamicalSystem( - std::make_unique( - std::move(system), wrapped)); + self.setDynamicalSystem(makeOwnedDynamicalSystem( + std::move(system))); }, py::arg("system"), "The solver keeps a Python reference that keeps this dynamics " @@ -456,9 +595,7 @@ void bind_solver(py::module_ &m) { "may produce unexpected behavior.") .def("set_objective", [](cddp::CDDP &self, py::object objective) { - auto *wrapped = objective.cast(); - self.setObjective(std::make_unique( - std::move(objective), wrapped)); + self.setObjective(makeOwnedObjective(std::move(objective))); }, py::arg("objective"), "The solver keeps a Python reference that keeps this objective " @@ -466,10 +603,9 @@ void bind_solver(py::module_ &m) { "produce unexpected behavior.") .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)); + makeOwnedConstraint( + std::move(constraint))); }, py::arg("name"), py::arg("constraint"), "The solver keeps a Python reference that keeps this path " @@ -477,10 +613,8 @@ void bind_solver(py::module_ &m) { "call may produce unexpected behavior.") .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)); + name, makeOwnedConstraint(std::move(constraint))); }, py::arg("name"), py::arg("constraint"), "The solver keeps a Python reference that keeps this terminal " @@ -502,14 +636,20 @@ void bind_solver(py::module_ &m) { py::call_guard()) .def("solve_by_name", [](cddp::CDDP &self, const std::string &solver_type) { - if (!isBuiltinSolverName(solver_type)) { - throw py::value_error( - "Unknown solver '" + solver_type + - "'. Supported solver names: " + builtinSolverList() + - "."); + if (!isKnownSolverName(solver_type)) { + throw py::value_error("Unknown solver '" + solver_type + + "'."); + } + cddp::CDDPSolution solution; + { + py::gil_scoped_release release; + solution = self.solve(solver_type); + } + if (isUnknownSolverStatus(solution.status_message)) { + throw py::value_error("Unknown solver '" + solver_type + + "'."); } - py::gil_scoped_release release; - return self.solve(solver_type); + return solution; }, py::arg("solver_type")) .def_property_readonly("initial_state", &cddp::CDDP::getInitialState) diff --git a/python/tests/test_constraints.py b/python/tests/test_constraints.py index ab30ed35..627c6dd4 100644 --- a/python/tests/test_constraints.py +++ b/python/tests/test_constraints.py @@ -1,6 +1,7 @@ """Test constraint construction and evaluation.""" import numpy as np import pycddp +import pytest class CountingAffineConstraint(pycddp.Constraint): @@ -111,3 +112,28 @@ def test_custom_python_constraint_with_solver(): assert counters["evaluate"] > 0 assert counters["state_jacobian"] > 0 assert counters["control_jacobian"] > 0 +def test_constraint_base_is_rejected_cleanly(): + dt = 0.1 + 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), 8, 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( + pycddp.QuadraticObjective( + np.eye(2), 0.1 * np.eye(1), 10.0 * np.eye(2), np.zeros(2), [], dt + ) + ) + + with pytest.raises(TypeError, match="Constraint is an abstract base class"): + solver.add_constraint("bad", pycddp.Constraint("bad")) diff --git a/python/tests/test_pendulum.py b/python/tests/test_pendulum.py index 4ea94807..4ced524e 100644 --- a/python/tests/test_pendulum.py +++ b/python/tests/test_pendulum.py @@ -76,3 +76,34 @@ def test_pendulum_logddp(): assert len(history.objective) == len(history.complementary_infeasibility) assert len(history.objective) == len(history.regularization) assert len(history.objective) == len(history.barrier_mu) + + +def test_pendulum_parallel_native_callbacks(): + dt = 0.05 + horizon = 40 + 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 = 50 + 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( + 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_common_solution_fields(solution, horizon, "CLDDP") diff --git a/python/tests/test_solver_errors.py b/python/tests/test_solver_errors.py index 46871051..83a82973 100644 --- a/python/tests/test_solver_errors.py +++ b/python/tests/test_solver_errors.py @@ -25,7 +25,15 @@ def test_solve_by_name_raises_for_unknown_solver(): solver.solve_by_name("NONEXISTENT") -def test_solve_by_name_accepts_builtin_solver(): +@pytest.mark.parametrize( + ("solver_name", "expected_solver_name"), + [ + ("CLDDP", "CLDDP"), + ("CLCDDP", "CLDDP"), + ("LOGDDP", "LogDDP"), + ], +) +def test_solve_by_name_accepts_core_aliases(solver_name, expected_solver_name): dt = 0.05 horizon = 20 x0 = np.array([np.pi, 0.0]) @@ -49,9 +57,9 @@ def test_solve_by_name_accepts_builtin_solver(): "ctrl", pycddp.ControlConstraint(np.array([-50.0]), np.array([50.0])) ) - solution = solver.solve_by_name("CLDDP") + solution = solver.solve_by_name(solver_name) - assert solution.solver_name == "CLDDP" + assert solution.solver_name == expected_solver_name assert solution.status_message assert len(solution.state_trajectory) == horizon + 1 @@ -65,6 +73,18 @@ def test_set_initial_trajectory_requires_dynamical_system(): solver.set_initial_trajectory(X, U) +def test_set_dynamical_system_rejects_abstract_base(): + solver = _make_solver() + + with pytest.raises(TypeError, match="DynamicalSystem is an abstract base class"): + solver.set_dynamical_system(pycddp.DynamicalSystem(2, 1, 0.1)) + + +def test_set_objective_rejects_abstract_base(): + with pytest.raises(TypeError, match="No constructor defined"): + pycddp.Objective() + + def test_set_initial_trajectory_rejects_bad_lengths(): solver = _make_solver() solver.set_dynamical_system(pycddp.Pendulum(0.1)) @@ -109,8 +129,17 @@ def test_import_error_message_is_actionable(tmp_path): shutil.copy(source_dir / "_version.py", package_dir / "_version.py") proc = subprocess.run( - [sys.executable, "-s", "-c", "import pycddp"], - env={"PYTHONPATH": str(tmp_path)}, + [ + sys.executable, + "-I", + "-S", + "-c", + ( + "import sys; " + f"sys.path.insert(0, {str(tmp_path)!r}); " + "import pycddp" + ), + ], capture_output=True, text=True, check=False, From 198032ea60c5ee711352d89f56cffaf9901f1051 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Wed, 1 Apr 2026 15:32:20 -0400 Subject: [PATCH 14/17] Fix installed include path export Export both installed include roots so downstream consumers can use the public cddp-cpp header layout while existing internal quoted includes keep working. Strengthen the package smoke test to compile against installed cddp-cpp headers directly. --- CMakeLists.txt | 1 + tests/package_smoke/main.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 47c7cb44..3356a3c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,6 +186,7 @@ endif() target_include_directories(${PROJECT_NAME} PUBLIC $ + $ $ ) diff --git a/tests/package_smoke/main.cpp b/tests/package_smoke/main.cpp index f443ec45..f8cfc79b 100644 --- a/tests/package_smoke/main.cpp +++ b/tests/package_smoke/main.cpp @@ -1,3 +1,8 @@ -#include "cddp.hpp" +#include +#include -int main() { return 0; } +int main() { + const auto finite = cddp::finite_difference_jacobian( + [](const Eigen::VectorXd &x) { return x; }, Eigen::VectorXd::Zero(1)); + return finite.rows() == 1 ? 0 : 1; +} From e76b2a0cd3b98810952568a9cfc7c03bdd28af3c Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Wed, 1 Apr 2026 15:46:02 -0400 Subject: [PATCH 15/17] Clean up repository boilerplate text Tighten repository-facing prose and remove generic template language.\nKeep the changes limited to documentation and comments without affecting build behavior. --- .../PR_TEMPLATE/pull_request_template.md | 18 +++++-------- README.md | 26 +++++++++---------- examples/CMakeLists.txt | 6 +---- 3 files changed, 20 insertions(+), 30 deletions(-) diff --git a/.github/workflows/PR_TEMPLATE/pull_request_template.md b/.github/workflows/PR_TEMPLATE/pull_request_template.md index d3f7fb1c..9ea67c13 100644 --- a/.github/workflows/PR_TEMPLATE/pull_request_template.md +++ b/.github/workflows/PR_TEMPLATE/pull_request_template.md @@ -1,20 +1,14 @@ ## Summary - -- -- + - ## Changes - -- **Component/Feature**: Description of changes -- **Another Component**: Description of changes + - ## Test Plan - -- [ ] Test item 1 -- [ ] Test item 2 -- [ ] Test item 3 + +- [ ] -## Additional Notes - +## Notes + diff --git a/README.md b/README.md index 44201db5..825d4f64 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Constrained Differential Dynamic Programming (CDDP) solver in C++ CDDP IN CPP -**This library is under active development.** Star the repo :star: to stay updated on its progress and eventual release. I welcome any suggestions as I'm building this library to gain practical C++ experience. +This repository contains a C++ implementation of constrained differential dynamic programming (CDDP) and related solvers for trajectory optimization and model predictive control. ## Overview This is an optimal control solver library using constrained differential dynamic programming (CDDP) written in C++. This library is particularly useful for mobile robot trajectory optimization and model predictive control (MPC). @@ -35,9 +35,9 @@ The default C++ build currently includes a barrier-strategy comparison example: ./examples/test_barrier_strategies ``` -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. +Several visualization-focused C++ examples remain in the repository, but they +are not part of the default build. Use the Python bindings for plotting and +notebook workflows. ## Installation ### Dependencies @@ -91,17 +91,17 @@ If you use this work in an academic context, please cite this repository. This project is licensed under the Apache License 2.0 - see the [LICENSE](LICENSE) file for details. ## Collaboration -Contributions to this CDDP solver library are welcome! Whether you're interested in fixing bugs, adding new features, improving documentation, or suggesting new example applications, your input is valuable. +Contributions are welcome. If you'd like to contribute: -1. **Fork the repository.** -2. **Create a new branch** for your feature or bug fix: `git checkout -b feature/your-feature-name` or `git checkout -b fix/your-bug-fix`. -3. **Make your changes** and ensure they are well-tested. -4. **Commit your changes:** `git commit -m 'Add some amazing feature'`. -5. **Push to the branch:** `git push origin feature/your-feature-name`. -6. **Open a Pull Request** against the `main` (or `master`) branch. - -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. +1. Fork the repository. +2. Create a branch for your change, for example `feature/your-feature-name` or `fix/your-bug-fix`. +3. Make the change and add or update tests as needed. +4. Commit with a descriptive message. +5. Push the branch. +6. Open a pull request against `master`. + +Use GitHub issues for bug reports, questions, or proposed changes. ## TODO * improve python binding ergonomics diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 931dc5ee..f34c5263 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -12,11 +12,7 @@ # See the License for the specific language governing permissions and # 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. +# Build only the maintained example used in the default C++ configuration. add_executable(test_barrier_strategies test_barrier_strategies.cpp) target_link_libraries(test_barrier_strategies cddp) From 18d70176349703dfa0f4c14d736e94161935b0fb Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Wed, 1 Apr 2026 17:31:45 -0400 Subject: [PATCH 16/17] Add Python dependency guard workflow Add a pull request workflow that reviews Python dependency changes. Enforce a conservative policy for uv lockfile sources and require explicit allowlisting for new top-level dependencies. --- .../workflows/python-dependency-guard.yaml | 41 ++++ security/python-direct-deps-allowlist.txt | 8 + tools/check_python_dependency_policy.py | 222 ++++++++++++++++++ 3 files changed, 271 insertions(+) create mode 100644 .github/workflows/python-dependency-guard.yaml create mode 100644 security/python-direct-deps-allowlist.txt create mode 100644 tools/check_python_dependency_policy.py diff --git a/.github/workflows/python-dependency-guard.yaml b/.github/workflows/python-dependency-guard.yaml new file mode 100644 index 00000000..374f4729 --- /dev/null +++ b/.github/workflows/python-dependency-guard.yaml @@ -0,0 +1,41 @@ +name: Python Dependency Guard + +on: + pull_request: + branches: [ master ] + paths: + - "pyproject.toml" + - "uv.lock" + - "security/python-direct-deps-allowlist.txt" + - "tools/check_python_dependency_policy.py" + +permissions: + contents: read + +jobs: + policy: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: "3.12" + + - name: Enforce dependency policy + run: python tools/check_python_dependency_policy.py --base-ref "origin/${{ github.base_ref }}" + + dependency-review: + runs-on: ubuntu-latest + permissions: + contents: read + pull-requests: write + steps: + - name: Check dependency review + uses: actions/dependency-review-action@v4 + with: + fail-on-severity: high diff --git a/security/python-direct-deps-allowlist.txt b/security/python-direct-deps-allowlist.txt new file mode 100644 index 00000000..4b712474 --- /dev/null +++ b/security/python-direct-deps-allowlist.txt @@ -0,0 +1,8 @@ +# New top-level Python dependencies must be added here in the same PR. +# Keep names normalized to package names, one per line. + +matplotlib +numpy +pybind11 +pytest +scikit-build-core diff --git a/tools/check_python_dependency_policy.py b/tools/check_python_dependency_policy.py new file mode 100644 index 00000000..c4fb5b49 --- /dev/null +++ b/tools/check_python_dependency_policy.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python3 +"""Enforce a conservative dependency policy for Python packaging files.""" + +from __future__ import annotations + +import argparse +import re +import subprocess +import sys +import tomllib +from pathlib import Path +from typing import Iterable + + +REPO_ROOT = Path(__file__).resolve().parents[1] +PYPROJECT_PATH = REPO_ROOT / "pyproject.toml" +LOCK_PATH = REPO_ROOT / "uv.lock" +ALLOWLIST_PATH = REPO_ROOT / "security" / "python-direct-deps-allowlist.txt" +PYPI_SIMPLE = "https://pypi.org/simple" +NAME_RE = re.compile(r"\s*([A-Za-z0-9][A-Za-z0-9._-]*)") + + +def normalize_name(name: str) -> str: + return re.sub(r"[-_.]+", "-", name).lower() + + +def extract_requirement_name(spec: str) -> str: + match = NAME_RE.match(spec) + if not match: + raise ValueError(f"Could not parse requirement name from {spec!r}") + return normalize_name(match.group(1)) + + +def is_direct_reference(spec: str) -> bool: + lowered = spec.lower() + return ( + " @" in spec + or lowered.startswith(("git+", "http://", "https://", "file:")) + or "git+" in lowered + or "://" in spec + ) + + +def load_toml(path: Path) -> dict: + with path.open("rb") as fh: + return tomllib.load(fh) + + +def load_allowlist(path: Path) -> set[str]: + allowed: set[str] = set() + for line in path.read_text().splitlines(): + line = line.strip() + if not line or line.startswith("#"): + continue + allowed.add(normalize_name(line)) + return allowed + + +def iter_dependency_specs(pyproject: dict) -> Iterable[tuple[str, str]]: + build_system = pyproject.get("build-system", {}) + for spec in build_system.get("requires", []): + yield ("build-system.requires", spec) + + project = pyproject.get("project", {}) + for spec in project.get("dependencies", []): + yield ("project.dependencies", spec) + + for group, specs in project.get("optional-dependencies", {}).items(): + for spec in specs: + yield (f"project.optional-dependencies.{group}", spec) + + for group, specs in pyproject.get("dependency-groups", {}).items(): + for spec in specs: + yield (f"dependency-groups.{group}", spec) + + +def collect_direct_dependency_names(pyproject: dict) -> set[str]: + names: set[str] = set() + for _, spec in iter_dependency_specs(pyproject): + names.add(extract_requirement_name(spec)) + return names + + +def load_base_pyproject(base_ref: str) -> dict | None: + result = subprocess.run( + ["git", "show", f"{base_ref}:pyproject.toml"], + cwd=REPO_ROOT, + check=False, + capture_output=True, + text=True, + ) + if result.returncode != 0: + return None + return tomllib.loads(result.stdout) + + +def load_base_lock(base_ref: str) -> dict | None: + result = subprocess.run( + ["git", "show", f"{base_ref}:uv.lock"], + cwd=REPO_ROOT, + check=False, + capture_output=True, + text=True, + ) + if result.returncode != 0: + return None + return tomllib.loads(result.stdout) + + +def validate_dependency_specs(pyproject: dict) -> list[str]: + errors: list[str] = [] + for source, spec in iter_dependency_specs(pyproject): + try: + extract_requirement_name(spec) + except ValueError as exc: + errors.append(f"{source}: {exc}") + continue + + if is_direct_reference(spec): + errors.append( + f"{source}: direct URL/git/file references are not allowed: {spec!r}" + ) + return errors + + +def validate_lockfile(lock_data: dict) -> list[str]: + errors: list[str] = [] + for package in lock_data.get("package", []): + name = package.get("name", "") + source = package.get("source") + if source is not None: + if source.get("editable") == ".": + continue + + registry = source.get("registry") + if registry != PYPI_SIMPLE: + errors.append( + f"uv.lock package {name!r} uses non-PyPI source {source!r}" + ) + + sdist = package.get("sdist") + if sdist and "hash" not in sdist: + errors.append(f"uv.lock package {name!r} is missing an sdist hash") + + for wheel in package.get("wheels", []): + if "hash" not in wheel: + errors.append(f"uv.lock package {name!r} has a wheel without a hash") + return errors + + +def collect_lock_package_names(lock_data: dict | None) -> set[str]: + if not lock_data: + return set() + return { + normalize_name(package["name"]) + for package in lock_data.get("package", []) + if "name" in package + } + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument( + "--base-ref", + default="origin/master", + help="Git ref used as the review baseline.", + ) + args = parser.parse_args() + + pyproject = load_toml(PYPROJECT_PATH) + lock_data = load_toml(LOCK_PATH) + allowlist = load_allowlist(ALLOWLIST_PATH) + base_pyproject = load_base_pyproject(args.base_ref) + base_lock = load_base_lock(args.base_ref) + + errors = [] + errors.extend(validate_dependency_specs(pyproject)) + errors.extend(validate_lockfile(lock_data)) + + current_direct = collect_direct_dependency_names(pyproject) + base_direct = collect_direct_dependency_names(base_pyproject) if base_pyproject else set() + added_direct = sorted(current_direct - base_direct) + unapproved_direct = [name for name in added_direct if name not in allowlist] + if unapproved_direct: + errors.append( + "New top-level Python dependencies must be explicitly allowlisted in " + f"{ALLOWLIST_PATH.relative_to(REPO_ROOT)}: " + + ", ".join(unapproved_direct) + ) + + current_locked = collect_lock_package_names(lock_data) + base_locked = collect_lock_package_names(base_lock) + added_locked = sorted(current_locked - base_locked) + + if errors: + print("Dependency policy check failed:", file=sys.stderr) + for error in errors: + print(f" - {error}", file=sys.stderr) + if added_direct: + print( + "Direct dependencies added in this change: " + + ", ".join(added_direct), + file=sys.stderr, + ) + if added_locked: + print( + "New locked packages introduced in this change: " + + ", ".join(added_locked), + file=sys.stderr, + ) + return 1 + + print("Dependency policy check passed.") + if added_direct: + print("Direct dependencies added for review: " + ", ".join(added_direct)) + if added_locked: + print("New locked packages introduced for review: " + ", ".join(added_locked)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From 7a14bf878a6c86177a4b56a615120b3859ba60ef Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Thu, 2 Apr 2026 14:52:07 -0400 Subject: [PATCH 17/17] Fix missing uv.lock handling in dependency guard Treat uv.lock as optional when the repository does not track it.\nStill fail if the base ref had a lockfile and the current change removes it. --- tools/check_python_dependency_policy.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/tools/check_python_dependency_policy.py b/tools/check_python_dependency_policy.py index c4fb5b49..14db998e 100644 --- a/tools/check_python_dependency_policy.py +++ b/tools/check_python_dependency_policy.py @@ -46,6 +46,12 @@ def load_toml(path: Path) -> dict: return tomllib.load(fh) +def load_optional_toml(path: Path) -> dict | None: + if not path.exists(): + return None + return load_toml(path) + + def load_allowlist(path: Path) -> set[str]: allowed: set[str] = set() for line in path.read_text().splitlines(): @@ -168,14 +174,20 @@ def main() -> int: args = parser.parse_args() pyproject = load_toml(PYPROJECT_PATH) - lock_data = load_toml(LOCK_PATH) + lock_data = load_optional_toml(LOCK_PATH) allowlist = load_allowlist(ALLOWLIST_PATH) base_pyproject = load_base_pyproject(args.base_ref) base_lock = load_base_lock(args.base_ref) errors = [] errors.extend(validate_dependency_specs(pyproject)) - errors.extend(validate_lockfile(lock_data)) + if lock_data is not None: + errors.extend(validate_lockfile(lock_data)) + elif base_lock is not None: + errors.append( + f"{LOCK_PATH.relative_to(REPO_ROOT)} is missing in the current checkout " + "but exists in the base ref" + ) current_direct = collect_direct_dependency_names(pyproject) base_direct = collect_direct_dependency_names(base_pyproject) if base_pyproject else set()