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/.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/.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..3356a3c5 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 @@ -43,6 +45,7 @@ endif() # Options option(CDDP_CPP_BUILD_TESTS "Whether to build tests." ON) +option(CDDP_CPP_BUILD_EXAMPLES "Whether to build examples." ON) # CasADi Configuration option(CDDP_CPP_CASADI "Whether to use CasADi" OFF) @@ -92,16 +95,6 @@ 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) -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") @@ -116,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 @@ -172,16 +166,19 @@ 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} - $,Eigen3::Eigen,> - matplot - autodiff -) +# 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} + PUBLIC + $,Eigen3::Eigen,> + autodiff +) if(NOT Eigen3_FOUND) target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS}) @@ -189,12 +186,13 @@ endif() 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 @@ -241,7 +239,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) @@ -258,11 +256,52 @@ 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(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/Dockerfile b/Dockerfile index b52c2e3c..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/* @@ -31,15 +28,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 diff --git a/README.md b/README.md index bb378d9f..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). @@ -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 +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 @@ -157,21 +53,9 @@ 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 +git clone https://github.com/astomodynamics/cddp-cpp cd cddp-cpp mkdir build && cd build cmake .. @@ -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: @@ -208,22 +91,22 @@ 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. +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`. -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. +Use GitHub issues for bug reports, questions, or proposed changes. ## 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 new file mode 100644 index 00000000..4b488299 --- /dev/null +++ b/cmake/cddpConfig.cmake.in @@ -0,0 +1,8 @@ +@PACKAGE_INIT@ + +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..f34c5263 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -12,112 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -# CMakeLists.txt for the CDDP examples +# 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) - -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 1bfe61ad..27e6fccf 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -57,6 +57,4 @@ #include "dynamics_model/quaternion_attitude.hpp" #include "dynamics_model/mrp_attitude.hpp" -#include "matplot/matplot.h" - #endif // CDDP_HPP 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/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..967ea699 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,23 @@ +[build-system] +requires = ["scikit-build-core>=0.8", "pybind11>=2.12,<3"] +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"] +wheel.packages = ["python/pycddp"] diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 00000000..8a12a13d --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,34 @@ +find_package(Python3 COMPONENTS Interpreter Development.Module REQUIRED) + +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) +install( + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/__init__.py + ${CMAKE_CURRENT_SOURCE_DIR}/pycddp/_version.py + DESTINATION pycddp +) diff --git a/python/pycddp/__init__.py b/python/pycddp/__init__.py new file mode 100644 index 00000000..9ea7ddd4 --- /dev/null +++ b/python/pycddp/__init__.py @@ -0,0 +1,89 @@ +"""Python bindings for Constrained Differential Dynamic Programming. + +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, + + # 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, + + ) +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__ + +__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..d1ac3c0b --- /dev/null +++ b/python/src/bind_constraints.cpp @@ -0,0 +1,160 @@ +#include +#include +#include + +#include "cddp_core/constraint.hpp" + +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") + .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) + .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("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); + + 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..4efa7efd --- /dev/null +++ b/python/src/bind_dynamics.cpp @@ -0,0 +1,264 @@ +#include +#include +#include + +#include "cddp_core/dynamical_system.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/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/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/spacecraft_roe.hpp" +#include "dynamics_model/spacecraft_twobody.hpp" +#include "dynamics_model/unicycle.hpp" +#include "dynamics_model/usv_3dof.hpp" + +namespace py = pybind11; + +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 { + // 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); + } + + 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_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_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_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_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_NAME(std::vector, + cddp::DynamicalSystem, "get_cross_hessian", + getCrossHessian, state, control, time); + } +}; + +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, + 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); + + 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..9c78f09f --- /dev/null +++ b/python/src/bind_objective.cpp @@ -0,0 +1,64 @@ +#include +#include +#include + +#include "cddp_core/objective.hpp" + +namespace py = pybind11; + +// 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) { + 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..747e70ab --- /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 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) + .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 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 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) + .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..74b6b180 --- /dev/null +++ b/python/src/bind_solver.cpp @@ -0,0 +1,663 @@ +#include +#include +#include + +#include +#include +#include + +#include "cddp_core/cddp_core.hpp" + +namespace py = pybind11; + +namespace { + +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; + } + + 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; +} + +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, + 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 &e) { + throw py::value_error( + std::string("set_initial_trajectory failed while querying dimensions " + "(is a dynamical system set?): ") + e.what()); + } + + 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 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 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, + bool requires_gil) + : cddp::DynamicalSystem(wrapped->getStateDim(), wrapped->getControlDim(), + wrapped->getTimestep(), + wrapped->getIntegrationType()), + owner_(std::move(owner)), wrapped_(wrapped), + requires_gil_(requires_gil) {} + + ~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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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 { + 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, + bool requires_gil) + : owner_(std::move(owner)), wrapped_(wrapped), + requires_gil_(requires_gil) {} + + ~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 { + return callWrapped(requires_gil_, + [&] { return wrapped_->evaluate(states, controls); }); + } + + double running_cost(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->running_cost(state, control, index); + }); + } + + double terminal_cost(const Eigen::VectorXd &final_state) const override { + 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 { + 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 { + return callWrapped(requires_gil_, [&] { + return wrapped_->getRunningCostControlGradient(state, control, + index); + }); + } + + Eigen::VectorXd + getFinalCostGradient(const Eigen::VectorXd &final_state) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->getFinalCostGradient(final_state); + }); + } + + Eigen::MatrixXd + getRunningCostStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + 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 { + 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 { + return callWrapped(requires_gil_, [&] { + return wrapped_->getRunningCostCrossHessian(state, control, index); + }); + } + + Eigen::MatrixXd + getFinalCostHessian(const Eigen::VectorXd &final_state) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->getFinalCostHessian(final_state); + }); + } + + Eigen::VectorXd getReferenceState() const override { + return callWrapped(requires_gil_, + [&] { return wrapped_->getReferenceState(); }); + } + + std::vector getReferenceStates() const override { + return callWrapped(requires_gil_, + [&] { return wrapped_->getReferenceStates(); }); + } + + void setReferenceState(const Eigen::VectorXd &reference_state) override { + callWrapped(requires_gil_, + [&] { wrapped_->setReferenceState(reference_state); }); + } + + void setReferenceStates( + const std::vector &reference_states) override { + 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, + bool requires_gil) + : cddp::Constraint(wrapped->getName()), owner_(std::move(owner)), + wrapped_(wrapped), requires_gil_(requires_gil) {} + + ~PythonBackedConstraint() override { + try { + if (Py_IsInitialized()) { + py::gil_scoped_acquire gil; + owner_.release().dec_ref(); + } + } catch (...) {} + } + + int getDualDim() const override { + return callWrapped(requires_gil_, [&] { return wrapped_->getDualDim(); }); + } + + Eigen::VectorXd evaluate(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->evaluate(state, control, index); + }); + } + + Eigen::VectorXd getLowerBound() const override { + return callWrapped(requires_gil_, + [&] { return wrapped_->getLowerBound(); }); + } + + Eigen::VectorXd getUpperBound() const override { + return callWrapped(requires_gil_, + [&] { return wrapped_->getUpperBound(); }); + } + + Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + 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 { + return callWrapped(requires_gil_, [&] { + return wrapped_->getControlJacobian(state, control, index); + }); + } + + double computeViolation(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->computeViolation(state, control, index); + }); + } + + double computeViolationFromValue(const Eigen::VectorXd &g) const override { + return callWrapped(requires_gil_, [&] { + return wrapped_->computeViolationFromValue(g); + }); + } + + Eigen::VectorXd getCenter() const override { + return callWrapped(requires_gil_, [&] { return wrapped_->getCenter(); }); + } + + std::vector getStateHessian(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + int index) const override { + 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 { + 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 { + 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) { + 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); + + 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); + + 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")) + .def("set_dynamical_system", + [](cddp::CDDP &self, py::object system) { + self.setDynamicalSystem(makeOwnedDynamicalSystem( + std::move(system))); + }, + py::arg("system"), + "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) { + self.setObjective(makeOwnedObjective(std::move(objective))); + }, + py::arg("objective"), + "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) { + self.addPathConstraint(name, + makeOwnedConstraint( + std::move(constraint))); + }, + py::arg("name"), py::arg("constraint"), + "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) { + self.addTerminalConstraint( + name, makeOwnedConstraint(std::move(constraint))); + }, + py::arg("name"), py::arg("constraint"), + "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", + &cddp::CDDP::removeTerminalConstraint, py::arg("name")) + .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::call_guard()) + .def("solve_by_name", + [](cddp::CDDP &self, const std::string &solver_type) { + 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 + + "'."); + } + return solution; + }, + 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..627c6dd4 --- /dev/null +++ b/python/tests/test_constraints.py @@ -0,0 +1,139 @@ +"""Test constraint construction and evaluation.""" +import numpy as np +import pycddp +import pytest + + +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 + 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 + + +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 +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_custom_dynamics.py b/python/tests/test_custom_dynamics.py new file mode 100644 index 00000000..1648e65e --- /dev/null +++ b/python/tests/test_custom_dynamics.py @@ -0,0 +1,160 @@ +"""Test custom dynamics defined in Python via the trampoline.""" +import numpy as np +import pytest +import threading +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 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))] + + +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 + ) + + +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 + 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.""" + dt = 0.1 + sys = DoubleIntegrator(dt) + x = np.array([0.0, 1.0]) + u = np.array([0.5]) + + 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.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) + + +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_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_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..4ced524e --- /dev/null +++ b/python/tests/test_pendulum.py @@ -0,0 +1,109 @@ +"""Integration test: solve a pendulum swing-up problem.""" +import numpy as np +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 + 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_common_solution_fields(solution, horizon, "CLDDP") + assert np.linalg.norm(solution.state_trajectory[-1] - xref) < np.linalg.norm(x0 - xref) + + +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 + 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)) + 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_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) + + +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 new file mode 100644 index 00000000..83a82973 --- /dev/null +++ b/python/tests/test_solver_errors.py @@ -0,0 +1,149 @@ +"""Regression tests for Python-facing solver validation.""" +import pathlib +import numpy as np +import pytest +import shutil +import subprocess +import sys + +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") + + +@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]) + 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(solver_name) + + assert solution.solver_name == expected_solver_name + 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)] + U = [np.zeros(1) for _ in range(solver.horizon)] + + with pytest.raises(ValueError, match="is a dynamical system set"): + 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)) + + 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) + + +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, + "-I", + "-S", + "-c", + ( + "import sys; " + f"sys.path.insert(0, {str(tmp_path)!r}); " + "import pycddp" + ), + ], + 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/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/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index f3438ed0..870a5e46 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() @@ -297,6 +302,14 @@ 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); } } @@ -349,12 +362,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/CMakeLists.txt b/tests/CMakeLists.txt index da67e7de..68fb1003 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) @@ -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/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index ce3f8a18..9aee9940 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -110,6 +110,99 @@ 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 {} +}; + +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(); @@ -268,6 +361,54 @@ 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_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 @@ -351,4 +492,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 +} diff --git a/tests/cddp_core/test_clddp_solver.cpp b/tests/cddp_core/test_clddp_solver.cpp index 8e6cfb4d..d85ffe91 100644 --- a/tests/cddp_core/test_clddp_solver.cpp +++ b/tests/cddp_core/test_clddp_solver.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include #include @@ -827,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 a12b4ec4..dc8e75fa 100644 --- a/tests/cddp_core/test_constraint.cpp +++ b/tests/cddp_core/test_constraint.cpp @@ -17,7 +17,6 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" #include "cddp-cpp/cddp_core/constraint.hpp" -#include #include TEST(ControlConstraintTest, Evaluate) { @@ -233,88 +232,6 @@ TEST(LinearConstraintTest, Evaluate) { ASSERT_NEAR(constraint_value(0), 0.0, 1e-6); } -// New test case for LinearConstraint visualization -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; -} - // New test suite for SecondOrderConeConstraint TEST(SecondOrderConeConstraintTest, Evaluate) { Eigen::Vector3d origin(0.0, 0.0, 0.0); @@ -393,141 +310,6 @@ TEST(SecondOrderConeConstraintTest, Gradients) { ASSERT_TRUE(control_jacobian.isApprox(Eigen::MatrixXd::Zero(1, control.size()))); } -// New test case for visualization -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; -} - 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); @@ -620,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 7e9b99e6..4e1d5019 100644 --- a/tests/cddp_core/test_ipddp_solver.cpp +++ b/tests/cddp_core/test_ipddp_solver.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include #include @@ -822,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 49728d5b..a028d342 100644 --- a/tests/cddp_core/test_logddp_solver.cpp +++ b/tests/cddp_core/test_logddp_solver.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include #include @@ -832,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 bec1a407..45c94bd5 100644 --- a/tests/cddp_core/test_msipddp_core.cpp +++ b/tests/cddp_core/test_msipddp_core.cpp @@ -24,9 +24,6 @@ #include "gtest/gtest.h" #include "cddp.hpp" -#include "matplot/matplot.h" - -using namespace matplot; // 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..dbee2157 100644 --- a/tests/cddp_core/test_msipddp_solver.cpp +++ b/tests/cddp_core/test_msipddp_solver.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include #include #include @@ -822,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 078039f5..e3039787 100644 --- a/tests/dynamics_model/test_attitude_dynamics.cpp +++ b/tests/dynamics_model/test_attitude_dynamics.cpp @@ -5,7 +5,6 @@ #include "cddp_core/helper.hpp" // For attitude conversions #include #include -#include // For plotting namespace cddp { @@ -211,7 +210,7 @@ namespace cddp } } - // --- Trajectory Comparison and Visualization --- + // --- Trajectory Comparison --- TEST_F(AttitudeDynamicsTest, TrajectoryComparison) { int num_steps = 500; @@ -266,202 +265,19 @@ namespace cddp omega_traj_quat_sim[k + 1] = current_state_quat.tail<3>(); } - // Plotting using matplotplusplus - using namespace matplot; - - // 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 5137129e..997b2bac 100644 --- a/tests/dynamics_model/test_mrp_attitude.cpp +++ b/tests/dynamics_model/test_mrp_attitude.cpp @@ -24,10 +24,7 @@ #include "cddp-cpp/dynamics_model/mrp_attitude.hpp" #include "cddp-cpp/cddp_core/helper.hpp" -#include "matplot/matplot.h" - using namespace cddp; -using namespace matplot; // Helper function for skew-symmetric matrix (double) Eigen::Matrix3d skew_double(const Eigen::Vector3d& v) { @@ -254,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); @@ -267,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); @@ -289,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 0b203e04..47a08a47 100644 --- a/tests/dynamics_model/test_spacecraft_linear.cpp +++ b/tests/dynamics_model/test_spacecraft_linear.cpp @@ -25,7 +25,6 @@ #include "gtest/gtest.h" #include "dynamics_model/spacecraft_linear.hpp" -#include "matplot/matplot.h" using namespace cddp; @@ -141,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 16ae1c9d..2bdeef5e 100644 --- a/tests/dynamics_model/test_spacecraft_linear_fuel.cpp +++ b/tests/dynamics_model/test_spacecraft_linear_fuel.cpp @@ -27,8 +27,6 @@ #include "dynamics_model/spacecraft_linear_fuel.hpp" #include "cddp.hpp" using namespace cddp; -using namespace matplot; -namespace plt = matplot; TEST(SpacecraftLinearFuelTest, DiscreteDynamics) { // Create an SpacecraftLinearFuel instance @@ -125,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 17507fa2..ea89b38f 100644 --- a/tests/dynamics_model/test_spacecraft_nonlinear.cpp +++ b/tests/dynamics_model/test_spacecraft_nonlinear.cpp @@ -21,8 +21,6 @@ #include #include -#include - #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -32,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, @@ -62,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) { @@ -167,90 +150,6 @@ TEST(SpacecraftNonlinearTest, AlfriendExample4_1_RelativeMotion) { 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)); - 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) { @@ -324,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 08a8e139..e724250d 100644 --- a/tests/dynamics_model/test_spacecraft_roe.cpp +++ b/tests/dynamics_model/test_spacecraft_roe.cpp @@ -12,7 +12,6 @@ #include #include #include -#include #include "dynamics_model/spacecraft_roe.hpp" #include "dynamics_model/spacecraft_linear.hpp" @@ -143,56 +142,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) 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..f8cfc79b --- /dev/null +++ b/tests/package_smoke/main.cpp @@ -0,0 +1,8 @@ +#include +#include + +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; +} 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 889fdc0c..00000000 --- a/tests/test_matplot.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include -#include -#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(); -} diff --git a/tools/check_python_dependency_policy.py b/tools/check_python_dependency_policy.py new file mode 100644 index 00000000..14db998e --- /dev/null +++ b/tools/check_python_dependency_policy.py @@ -0,0 +1,234 @@ +#!/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_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(): + 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_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)) + 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() + 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())