├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── VERSION ├── conftest.py ├── examples ├── dynamics │ ├── .gitignore │ ├── Makefile │ ├── README.md │ ├── include │ │ ├── dynamics_model.h │ │ ├── rigid_body.h │ │ ├── rollout_model.h │ │ └── types.h │ ├── requirements.txt │ ├── scripts │ │ ├── test_dynamics_model.py │ │ ├── test_rollout_model.py │ │ └── util.py │ └── src │ │ └── compile_model.cpp ├── pinocchio │ ├── .gitignore │ ├── Makefile │ ├── README.md │ ├── include │ │ └── inverse_dynamics_model.h │ ├── requirements.txt │ ├── scripts │ │ └── test_inverse_dynamics_model.py │ ├── src │ │ └── compile_model.cpp │ └── urdf │ │ ├── kinova.urdf │ │ └── ur5.urdf └── simple │ ├── .gitignore │ ├── Makefile │ ├── README.md │ ├── load_model.py │ └── model.cpp ├── include └── CppADCodeGenEigenPy │ ├── ADModel.h │ ├── CompiledModel.h │ ├── Util.h │ └── impl │ ├── ADModel.tpp │ └── CompiledModel.tpp ├── pyproject.toml ├── setup.py ├── src └── bindings.cpp └── tests ├── cpp_tests ├── BasicModelTest.cpp ├── LowOrderModelTest.cpp ├── MathFunctionsModelTest.cpp ├── MiscTest.cpp └── ParameterizedModelTest.cpp ├── helpers └── compile_models.cpp ├── include └── testing │ ├── Defs.h │ └── models │ ├── BasicTestModel.h │ ├── MathFunctionsTestModel.h │ └── ParameterizedTestModel.h └── python_tests ├── basic_model_test.py ├── low_order_model_test.py ├── math_functions_model_test.py └── parameterized_model_test.py /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.so 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adamheins/CppADCodeGenEigenPy/4f85ca831cc554484bbff946e2ffdf2c3e90db81/.gitmodules -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | file(STRINGS "VERSION" ver) 4 | message("Version: ${ver}") 5 | project(CppADCodeGenEigenPy LANGUAGES CXX VERSION ${ver}) 6 | 7 | set(CMAKE_CXX_STANDARD 11) 8 | set(CMAKE_CXX_STANDARD_REQUIRED True) 9 | 10 | # Note that cmake 3.12+ is required to find Python 11 | # find_package(Python3 COMPONENTS Interpreter Development REQUIRED) 12 | find_package(Boost COMPONENTS filesystem REQUIRED) 13 | find_package(Eigen3 3.3 REQUIRED) 14 | 15 | # install project header files 16 | install(DIRECTORY include/ DESTINATION include) 17 | 18 | include(FetchContent) 19 | 20 | # get googletest from github 21 | FetchContent_Declare( 22 | googletest 23 | URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip 24 | ) 25 | 26 | # avoid installing googletest along with this project 27 | option(INSTALL_GTEST "Enable installation of googletest." OFF) 28 | FetchContent_MakeAvailable(googletest) 29 | 30 | # build compiler for models used for the Python tests, then run it 31 | add_executable( 32 | python_test_model_compiler 33 | tests/helpers/compile_models.cpp 34 | ) 35 | target_include_directories(python_test_model_compiler PUBLIC include tests/include ${EIGEN3_INCLUDE_DIRS}) 36 | target_link_libraries( 37 | python_test_model_compiler 38 | dl 39 | ) 40 | add_custom_target( 41 | compile_python_test_models ALL 42 | COMMAND python_test_model_compiler ${CMAKE_CURRENT_BINARY_DIR} 43 | COMMENT "Compiling models for Python tests" 44 | VERBATIM 45 | ) 46 | 47 | # testing 48 | enable_testing() 49 | 50 | add_executable( 51 | model_tests 52 | tests/cpp_tests/MiscTest.cpp 53 | tests/cpp_tests/BasicModelTest.cpp 54 | tests/cpp_tests/ParameterizedModelTest.cpp 55 | tests/cpp_tests/MathFunctionsModelTest.cpp 56 | tests/cpp_tests/LowOrderModelTest.cpp 57 | ) 58 | target_include_directories(model_tests PUBLIC include tests/include ${EIGEN3_INCLUDE_DIRS}) 59 | target_link_libraries( 60 | model_tests 61 | gtest_main 62 | dl 63 | ${Boost_LIBRARIES} 64 | ) 65 | 66 | include(GoogleTest) 67 | gtest_discover_tests(model_tests) 68 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2021 Adam Heins 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CppADCodeGenEigenPy 2 | 3 | CppADCodeGen with an easy Eigen interface and Python bindings. 4 | 5 | This project has been tested on Ubuntu 16.04, 18.04, and 20.04. It may work on 6 | other Linux systems as well. 7 | 8 | ## Motivation 9 | 10 | I want to be able to prototype code in Python while making use of fast, 11 | compiled auto-differentiated code. Autodiff tools in Python, such as 12 | [JAX](https://github.com/google/jax), typically just-in-time (JIT) compile 13 | code rather than ahead-of-time compile it. I would like to avoid waiting for 14 | a JIT compile every time I run my script if the functions being differentiated 15 | haven't actually changed. 16 | 17 | The goal of CppADCodeGenEigenPy is to take the auto-diff and compilation 18 | capabilities of [CppADCodeGen](https://github.com/joaoleal/CppADCodeGen), wrap 19 | it up in an easy-to-use C++ interface, and provide built-in Python bindings to 20 | the compiled auto-differentiated model. The upshot is that the functions being 21 | differentiated need to be written in C++, but they need only be compiled once 22 | and they run very fast. 23 | 24 | This project was heavily inspired by larger frameworks that incorporate 25 | auto-diff functionality via CppADCodeGen for convenience; in particular, the 26 | [automatic_differentiation](https://github.com/leggedrobotics/ocs2/tree/main/ocs2_core/include/ocs2_core/automatic_differentiation) 27 | module from [OCS2](https://github.com/leggedrobotics/ocs2) was a big influence. 28 | 29 | An alterntive approach is something like 30 | [PyCppAd](https://github.com/Simple-Robotics/pycppad). Instead of just binding 31 | the resulting auto-differentiated model like CppADCodeGenEigenPy, PyCppAd 32 | actually binds the underlying CppADCodeGen types to enable the auto-diff + 33 | code-gen pipeline to be implemented directly from Python. 34 | 35 | ## How it works 36 | 37 | This project provides a simple interface to CppADCodeGen to define the required 38 | function in C++, which is then compiled (along with its automatically-computed 39 | derivatives) into a dynamic library. This dynamic library can then be easily used 40 | from C++ or imported into Python via bindings based on 41 | [pybind11](https://github.com/pybind/pybind11). 42 | 43 | CppADCodeGenEigenPy is built around two classes. The first is the `ADModel`, 44 | which defines the function to be auto-differentiated. It is compiled into a 45 | dynamic library which can then be loaded by the `CompiledModel` class. The 46 | `CompiledModel` can evaluate the function itself as well as its first and 47 | second derivatives. 48 | 49 | ## Install 50 | 51 | First ensure you have the required dependencies: 52 | * a compiler with C++11 support 53 | * [cmake](https://cmake.org/) version 3.12+ 54 | * [Eigen](https://eigen.tuxfamily.org/) version 3.3+ 55 | * [Boost](https://www.boost.org/) version 1.58+ (for the tests) 56 | * [CppADCodeGen](https://github.com/joaoleal/CppADCodeGen) 57 | 58 | Clone CppADCodeGenEigenPy: 59 | ``` 60 | git clone https://github.com/adamheins/CppADCodeGenEigenPy 61 | cd CppADCodeGenEigenPy 62 | ``` 63 | 64 | The main C++ part of CppADCodeGenEigenPy uses cmake. The library itself is 65 | header-only, so you need only do: 66 | ``` 67 | mkdir build 68 | cmake -S . -B build 69 | cmake --install build # may require sudo 70 | ``` 71 | 72 | To build and install the Python bindings, `pip` is used. Python 3 is strongly 73 | preferred. Note that `pip` version 10+ is required in order to parse 74 | `pyproject.toml`. Run 75 | ``` 76 | python -m pip install . 77 | ``` 78 | 79 | ## Tests 80 | 81 | To build the tests, run 82 | ``` 83 | cmake --build build 84 | ``` 85 | 86 | To run the C++ tests: 87 | ``` 88 | cd build 89 | ctest 90 | ``` 91 | 92 | To run the Python tests: 93 | ``` 94 | python -m pytest 95 | ``` 96 | Note that the tests assume that the name of the build directory is `build` in 97 | order to find the required dynamic libraries. If this is not the case, you can 98 | pass `--builddir NAME` to pytest to change it. 99 | 100 | 101 | ## Examples 102 | 103 | All examples are found in the [examples](examples) directory. These include the 104 | simple weighted sum-of-squares example described below, an example for 105 | differentiating the forward dynamics of a 3D rigid body, and an example of 106 | inverse dynamics using [Pinocchio](https://github.com/stack-of-tasks/pinocchio). 107 | 108 | ### Simple weighted sum-of-squares 109 | 110 | A minimal example of the C++ code to define and generate an auto-diff model 111 | would look something like: 112 | ```c++ 113 | #include 114 | #include 115 | 116 | namespace ad = CppADCodeGenEigenPy; 117 | 118 | // Our custom model extends the ad::ADModel class 119 | template 120 | struct ExampleModel : public ad::ADModel { 121 | using typename ad::ADModel::ADVector; 122 | 123 | // Generate the input used when differentiating the function 124 | ADVector input() const override { return ADVector::Ones(3); } 125 | 126 | // Generate parameters used when differentiating the function 127 | // This can be skipped if the function takes no parameters 128 | ADVector parameters() const override { return ADVector::Ones(3); } 129 | 130 | // Implement a weighted sum-of-squares function. 131 | ADVector function(const ADVector& input, 132 | const ADVector& parameters) const override { 133 | ADVector output(1); 134 | for (int i = 0; i < 3; ++i) { 135 | output(0) += 0.5 * parameters(i) * input(i) * input(i); 136 | } 137 | return output; 138 | } 139 | }; 140 | 141 | int main() { 142 | // Compile model named ExampleModel and save in the current directory; the 143 | // model is saved as a shared object file named libExampleModel.so 144 | ExampleModel().compile("ExampleModel", ".", 145 | ad::DerivativeOrder::First); 146 | } 147 | ``` 148 | 149 | Compile the code and generate the model: 150 | ``` 151 | g++ -std=c++11 -I/usr/local/include/eigen3 model.cpp -ldl -o make_model 152 | ./make_model 153 | ``` 154 | If you get an error about not being able to find an Eigen header, you may have 155 | to change the path to the Eigen include directory. 156 | 157 | This code can then be called from Python using: 158 | ```python 159 | import numpy as np 160 | from CppADCodeGenEigenPy import CompiledModel 161 | 162 | # note that the .so file extension not included on the second argument 163 | model = CompiledModel("ExampleModel", "libExampleModel") 164 | 165 | inputs = np.array([1, 2, 3]) 166 | params = np.ones(3) 167 | 168 | # compute model output and first derivative 169 | output = model.evaluate(inputs, params) 170 | jacobian = model.jacobian(inputs, params) 171 | 172 | print(f"output = {output}") 173 | print(f"jacobian = {jacobian}") 174 | ``` 175 | This C++ and Python code can be found [here](examples/simple). 176 | 177 | ## License 178 | 179 | [MIT](LICENSE) 180 | -------------------------------------------------------------------------------- /VERSION: -------------------------------------------------------------------------------- 1 | 0.2 2 | -------------------------------------------------------------------------------- /conftest.py: -------------------------------------------------------------------------------- 1 | def pytest_addoption(parser): 2 | parser.addoption("--builddir", action="store", default="build") 3 | -------------------------------------------------------------------------------- /examples/dynamics/.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | lib/ 3 | -------------------------------------------------------------------------------- /examples/dynamics/Makefile: -------------------------------------------------------------------------------- 1 | CCPP=g++ 2 | SRC_DIR=src 3 | BIN_DIR=bin 4 | LIB_DIR=lib 5 | COMPILER_BIN=$(BIN_DIR)/compile_model 6 | COMPILER_SRC=$(SRC_DIR)/compile_model.cpp 7 | 8 | INCLUDE_DIRS=-I/usr/include/eigen3 -I/usr/local/include/eigen3 -Iinclude 9 | CPP_FLAGS=-std=c++11 10 | 11 | # make the compiler for the model 12 | .PHONY: compiler 13 | compiler: 14 | @mkdir -p $(BIN_DIR) 15 | $(CCPP) $(INCLUDE_DIRS) $(CPP_FLAGS) $(COMPILER_SRC) -ldl -o $(COMPILER_BIN) 16 | 17 | # use the compiler to generate the model itself 18 | .PHONY: model 19 | model: 20 | @mkdir -p $(LIB_DIR) 21 | ./$(COMPILER_BIN) $(LIB_DIR) 22 | 23 | # clean up 24 | .PHONY: clean 25 | clean: 26 | @rm -rf $(BIN_DIR) 27 | @rm -rf $(LIB_DIR) 28 | -------------------------------------------------------------------------------- /examples/dynamics/README.md: -------------------------------------------------------------------------------- 1 | # CppADCodeGenEigenPy Dynamics Example 2 | 3 | This is an example of auto-differentiating functions related to rigid body 4 | dynamics. 5 | 6 | The [DynamicsModel](include/dynamics_model.h) shows how to differentiate basic 7 | forward dynamics computed using the Newton-Euler equations of motion. The 8 | [RolloutCostModel](include/rollout_model.h) shows how to differentiate a scalar 9 | cost function of the system state and input forward simulated over a time 10 | horizon, similar to what may be used for model predictive control. 11 | 12 | There are Python 13 | [scripts](scripts) 14 | that: 15 | 1. show how to use the auto-differentiated models from Python, and 16 | 2. compare the performance (both startup and runtime) with 17 | [JAX](https://github.com/google/jax), a popular Python autodiff library. 18 | 19 | ## Usage 20 | 21 | Compile the auto-differentiated models: 22 | ``` 23 | # this compiles the program to generate the model 24 | make compiler 25 | 26 | # use the program compiled by the above line to actually produce the model 27 | # (which is shared object .so file) 28 | make model 29 | ``` 30 | If you get an error about not being able to find an Eigen header, you may have 31 | to change the path to the Eigen include directory in the Makefile. 32 | 33 | Install required Python dependencies (note that Python 3 is expected): 34 | ``` 35 | pip install -r requirements.txt 36 | ``` 37 | 38 | Test out the models using the provided scripts: 39 | ``` 40 | python scripts/test_dynamics_model.py 41 | python scripts/test_rollout_model.py 42 | ``` 43 | These scripts print information about the execution time for the compiled model 44 | and the equivalent JAX model, and assert that both models produce equivalent 45 | results. 46 | 47 | On my system, loading the C++ `RolloutCostModel` takes under 1 millisecond, 48 | whereas the equivalent JAX model takes about 5 seconds, since it has to JIT 49 | compile each time the script is run. After the initial compilation, evaluating 50 | the Jacobians is also about an order of magnitude faster using the C++ model. 51 | -------------------------------------------------------------------------------- /examples/dynamics/include/dynamics_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "rigid_body.h" 8 | #include "types.h" 9 | 10 | namespace ad = CppADCodeGenEigenPy; 11 | 12 | template 13 | struct DynamicsModel : public ad::ADModel { 14 | using typename ad::ADModel::ADScalar; 15 | using typename ad::ADModel::ADVector; 16 | using typename ad::ADModel::ADMatrix; 17 | 18 | // Generate the input used when differentiating the function 19 | ADVector input() const override { 20 | return ADVector::Ones(STATE_DIM + INPUT_DIM); 21 | } 22 | 23 | // Generate parameters used when differentiating the function 24 | ADVector parameters() const override { 25 | ADScalar mass(1.0); 26 | Mat3 inertia = Mat3::Identity(); 27 | 28 | ADVector p(1 + inertia.size()); 29 | p << mass, Eigen::Map(inertia.data(), inertia.size()); 30 | return p; 31 | } 32 | 33 | ADScalar get_mass(const ADVector& parameters) const { 34 | return parameters(0); 35 | } 36 | 37 | Mat3 get_inertia(const ADVector& parameters) const { 38 | ADVector inertia_vec = parameters.tail(3 * 3); 39 | Eigen::Map> inertia(inertia_vec.data(), 3, 3); 40 | return inertia; 41 | } 42 | 43 | /** 44 | * Forward dynamics: compute acceleration from current state and force 45 | * input. 46 | */ 47 | ADVector function(const ADVector& input, 48 | const ADVector& parameters) const override { 49 | // Parameters are mass and inertia matrix of the body 50 | ADScalar mass = get_mass(parameters); 51 | Mat3 inertia = get_inertia(parameters); 52 | 53 | // Input is (state, system input) = (x, u) 54 | ADVector x = input.head(STATE_DIM); 55 | ADVector u = input.tail(INPUT_DIM); 56 | 57 | RigidBody body(mass, inertia); 58 | return body.forward_dynamics(x, u); 59 | } 60 | }; 61 | -------------------------------------------------------------------------------- /examples/dynamics/include/rigid_body.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "types.h" 6 | 7 | template 8 | struct RigidBody { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | 11 | RigidBody(const Scalar mass, const Mat3& inertia) 12 | : mass(mass), inertia(inertia), inertia_inv(inertia.inverse()) {} 13 | 14 | // Compute forward dynamics for the rigid body. Force inputs are assumed to 15 | // be applied in the inertial (world) frame. 16 | Vec6 forward_dynamics(const StateVec& x, 17 | const InputVec& u) const { 18 | // Compute inertia matrix rotated into the fixed world frame. 19 | Mat3 C_wo = orientation(x).toRotationMatrix(); 20 | Mat3 Iw = C_wo * inertia * C_wo.transpose(); 21 | Mat3 Iw_inv = C_wo * inertia_inv * C_wo.transpose(); 22 | 23 | Vec3 force = u.head(3); 24 | Vec3 torque = u.tail(3); 25 | Vec3 v = linear_velocity(x); 26 | Vec3 omega = angular_velocity(x); 27 | 28 | // Find acceleration from Newton-Euler equations 29 | Vec3 a = force / mass; 30 | Vec3 alpha = Iw_inv * (torque - omega.cross(Iw * omega)); 31 | 32 | Vec6 A; 33 | A << a, alpha; 34 | return A; 35 | } 36 | 37 | // Roll out the forward dynamics over num_steps time steps, each spaced dt 38 | // seconds apart. 39 | std::vector> rollout( 40 | const StateVec& x0, const std::vector>& us, 41 | const Scalar dt, const size_t num_steps) const { 42 | StateVec x = x0; 43 | std::vector> xs; 44 | for (InputVec u : us) { 45 | Vec6 A = forward_dynamics(x, u); 46 | 47 | // Integrate linear portion of state 48 | Vec3 r0 = position(x); 49 | Vec3 v0 = linear_velocity(x); 50 | Vec3 a = A.head(3); 51 | 52 | Vec3 r1 = r0 + dt * v0 + 0.5 * dt * dt * a; 53 | Vec3 v1 = v0 + dt * a; 54 | 55 | // Integrate rotation portion 56 | Eigen::Quaternion q0 = orientation(x); 57 | Vec3 omega0 = angular_velocity(x); 58 | Vec3 alpha = A.tail(3); 59 | Vec3 omega1 = omega0 + dt * alpha; 60 | 61 | // First term of the Magnus expansion 62 | Vec3 aa_vec = 0.5 * dt * (omega0 + omega1); 63 | 64 | // Map to a quaternion via exponential map (note this 65 | // implementation is not robust against numerical problems as angle 66 | // -> 0) 67 | Scalar angle = aa_vec.norm(); 68 | Vec3 axis = aa_vec / angle; 69 | Scalar c = cos(0.5 * angle); 70 | Scalar s = sin(0.5 * angle); 71 | 72 | Eigen::Quaternion qw; 73 | qw.coeffs() << s * axis, c; 74 | Eigen::Quaternion q1 = qw * q0; 75 | 76 | x << r1, q1.coeffs(), v1, omega1; 77 | xs.push_back(x); 78 | } 79 | return xs; 80 | } 81 | 82 | static StateVec zero_state() { 83 | StateVec x = StateVec::Zero(); 84 | x(6) = 1; // w of quaternion 85 | return x; 86 | } 87 | 88 | static Vec3 orientation_error(const Eigen::Quaternion& q, 89 | const Eigen::Quaternion& qd) { 90 | // Vector part of qd.inverse() * q. We implement it manually here to 91 | // avoid code branches (that check numerical stability) that AD cannot 92 | // deal with. 93 | return qd.w() * q.vec() - q.w() * qd.vec() - qd.vec().cross(q.vec()); 94 | } 95 | 96 | static StateErrorVec state_error(const StateVec& x, 97 | const StateVec& xd) { 98 | Eigen::Quaternion q = orientation(x); 99 | Eigen::Quaternion qd = orientation(xd); 100 | 101 | StateErrorVec e; 102 | e << position(xd) - position(x), orientation_error(q, qd), 103 | linear_velocity(xd) - linear_velocity(x), 104 | angular_velocity(xd) - angular_velocity(x); 105 | return e; 106 | } 107 | 108 | static Vec3 position(const StateVec& x) { 109 | return x.head(3); 110 | } 111 | 112 | static Eigen::Quaternion orientation(const StateVec& x) { 113 | Eigen::Quaternion q; 114 | q.coeffs() << x.segment(3, 4); 115 | return q; 116 | } 117 | 118 | // Get linear velocity component of the state 119 | static Vec3 linear_velocity(const StateVec& x) { 120 | return x.segment(7, 3); 121 | } 122 | 123 | // Get angular velocity component of the state 124 | static Vec3 angular_velocity(const StateVec& x) { 125 | return x.segment(10, 3); 126 | } 127 | 128 | Scalar mass; 129 | Mat3 inertia; 130 | Mat3 inertia_inv; 131 | }; 132 | -------------------------------------------------------------------------------- /examples/dynamics/include/rollout_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "rigid_body.h" 8 | #include "types.h" 9 | 10 | namespace ad = CppADCodeGenEigenPy; 11 | 12 | const double TIMESTEP = 0.1; 13 | const size_t NUM_TIME_STEPS = 10; 14 | const size_t NUM_INPUT = INPUT_DIM * NUM_TIME_STEPS; 15 | 16 | template 17 | struct RolloutCostModel : public ad::ADModel { 18 | using typename ad::ADModel::ADScalar; 19 | using typename ad::ADModel::ADVector; 20 | using typename ad::ADModel::ADMatrix; 21 | 22 | ADVector input() const override { 23 | // Input is the initial state and the force and torque at each timestep 24 | ADVector inp = ADVector::Ones(STATE_DIM + NUM_INPUT); 25 | inp.head(STATE_DIM) = RigidBody::zero_state(); 26 | return inp; 27 | } 28 | 29 | ADVector parameters() const override { 30 | // Parameters consist of mass (1), inertia (9), and desired states 31 | // (STATE_DIM * NUM_TIME_STEPS) 32 | ADScalar mass(1.0); 33 | Mat3 inertia = Mat3::Identity(); 34 | 35 | ADVector p(1 + 9 + STATE_DIM * NUM_TIME_STEPS); 36 | 37 | p(0) = mass; 38 | p.segment(1, 9) = Eigen::Map(inertia.data(), 9, 1); 39 | 40 | StateVec x0 = RigidBody::zero_state(); 41 | for (int i = 0; i < NUM_TIME_STEPS; ++i) { 42 | p.segment(10 + i * STATE_DIM, STATE_DIM) = x0; 43 | } 44 | return p; 45 | } 46 | 47 | static ADScalar get_mass(const ADVector& parameters) { 48 | return parameters(0); 49 | } 50 | 51 | static Mat3 get_inertia(const ADVector& parameters) { 52 | ADVector inertia_vec = parameters.segment(1, 3 * 3); 53 | Eigen::Map> inertia(inertia_vec.data(), 3, 3); 54 | return inertia; 55 | } 56 | 57 | static std::vector> get_desired_states( 58 | const ADVector& parameters) { 59 | const size_t start_idx = 10; 60 | std::vector> xds; 61 | for (size_t i = 0; i < NUM_TIME_STEPS; ++i) { 62 | xds.push_back( 63 | parameters.segment(start_idx + i * STATE_DIM, STATE_DIM)); 64 | } 65 | return xds; 66 | } 67 | 68 | static StateVec get_initial_state(const ADVector& input) { 69 | return input.head(STATE_DIM); 70 | } 71 | 72 | static std::vector> get_wrenches(const ADVector& input) { 73 | const size_t start_idx = STATE_DIM; 74 | std::vector> us; 75 | for (size_t i = 0; i < NUM_TIME_STEPS; ++i) { 76 | us.push_back( 77 | input.segment(start_idx + i * INPUT_DIM, INPUT_DIM)); 78 | } 79 | return us; 80 | } 81 | 82 | /** 83 | * Compute cost of the rollout. 84 | */ 85 | ADVector function(const ADVector& input, 86 | const ADVector& parameters) const override { 87 | ADScalar mass = get_mass(parameters); 88 | Mat3 inertia = get_inertia(parameters); 89 | 90 | std::vector> xds = get_desired_states(parameters); 91 | StateVec x0 = get_initial_state(input); 92 | std::vector> us = get_wrenches(input); 93 | 94 | // Do the rollout 95 | RigidBody body(mass, inertia); 96 | std::vector> xs = 97 | body.rollout(x0, us, ADScalar(TIMESTEP), NUM_TIME_STEPS); 98 | 99 | // Compute cost 100 | ADVector cost = ADVector::Zero(1); 101 | for (int i = 0; i < NUM_TIME_STEPS; ++i) { 102 | StateErrorVec x_err = 103 | RigidBody::state_error(xs[i], xds[i]); 104 | 105 | ADScalar step_cost = 106 | ADScalar(0.5) * (x_err.dot(x_err) + 107 | ADScalar(0.1) * us[i].dot(us[i])); 108 | cost(0) += step_cost; 109 | } 110 | return cost; 111 | } 112 | }; 113 | -------------------------------------------------------------------------------- /examples/dynamics/include/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | const size_t STATE_DIM = 7 + 6; 6 | const size_t INPUT_DIM = 6; 7 | 8 | template 9 | using Vec3 = Eigen::Matrix; 10 | 11 | template 12 | using Vec6 = Eigen::Matrix; 13 | 14 | template 15 | using StateVec = Eigen::Matrix; 16 | 17 | template 18 | using StateErrorVec = Eigen::Matrix; 19 | 20 | template 21 | using InputVec = Eigen::Matrix; 22 | 23 | // Note that we need to specify row-major here to match the 24 | // expected-convention for numpy, when passing in inertia parameters. 25 | template 26 | using Mat3 = Eigen::Matrix; 27 | -------------------------------------------------------------------------------- /examples/dynamics/requirements.txt: -------------------------------------------------------------------------------- 1 | jax==0.2.24 2 | numpy==1.21.3 3 | -------------------------------------------------------------------------------- /examples/dynamics/scripts/test_dynamics_model.py: -------------------------------------------------------------------------------- 1 | import os 2 | from functools import partial 3 | import time 4 | import timeit 5 | 6 | import numpy as np 7 | import jax 8 | import jax.numpy as jnp 9 | 10 | from CppADCodeGenEigenPy import CompiledModel 11 | 12 | import util 13 | 14 | 15 | SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__)) 16 | LIB_DIR = SCRIPT_DIR + "/../lib" 17 | DYNAMICS_MODEL_NAME = "DynamicsModel" 18 | ROLLOUT_COST_MODEL_NAME = "RolloutCostModel" 19 | 20 | STATE_DIM = 7 + 6 21 | INPUT_DIM = 6 22 | 23 | 24 | # It is convenient to create a wrapper around the C++ bindings to do things 25 | # like build the parameter vector. 26 | class CppDynamicsModelWrapper: 27 | """Wrapper around C++ bindings for DynamicsModel.""" 28 | 29 | def __init__(self, mass, inertia): 30 | lib_path = os.path.join(LIB_DIR, "lib" + DYNAMICS_MODEL_NAME) 31 | self._model = CompiledModel(DYNAMICS_MODEL_NAME, lib_path) 32 | 33 | self.mass = mass 34 | self.inertia = inertia 35 | 36 | def _params(self): 37 | """Create the parameter vector.""" 38 | return np.concatenate([[self.mass], self.inertia.reshape(9)]) 39 | 40 | def evaluate(self, x, u): 41 | """Compute acceleration using forward dynamics.""" 42 | inp = np.concatenate((x, u)) 43 | return self._model.evaluate(inp, self._params()) 44 | 45 | def jacobians(self, x, u): 46 | """Compute derivatives of forward dynamics w.r.t. state x and force input u.""" 47 | inp = np.concatenate((x, u)) 48 | J = self._model.jacobian(inp, self._params()) 49 | dfdx = J[:, :STATE_DIM] 50 | dfdu = J[:, STATE_DIM:] 51 | return dfdx, dfdu 52 | 53 | 54 | class JaxDynamicsModel: 55 | """Equivalent rigid body dynamics model in Python with JAX.""" 56 | 57 | def __init__(self, mass, inertia): 58 | self.mass = mass 59 | self.inertia = inertia 60 | self.inertia_inv = jnp.linalg.inv(inertia) 61 | 62 | self.dfdx = jax.jit(jax.jacfwd(self.evaluate, argnums=0)) 63 | self.dfdu = jax.jit(jax.jacfwd(self.evaluate, argnums=1)) 64 | 65 | @partial(jax.jit, static_argnums=(0,)) 66 | def evaluate(self, x, u): 67 | """Compute forward dynamics. 68 | 69 | Returns the acceleration based on the provided state x and force input 70 | u. 71 | """ 72 | f, τ = u[:3], u[3:] 73 | _, q, v, ω = util.decompose_state(x) 74 | 75 | C_wo = util.quaternion_to_rotation_matrix(q) 76 | Iw = C_wo @ self.inertia @ C_wo.T 77 | Iw_inv = C_wo @ self.inertia_inv @ C_wo.T 78 | 79 | a = f / self.mass 80 | α = Iw_inv @ (τ - jnp.cross(ω, Iw @ ω)) 81 | 82 | return jnp.concatenate((a, α)) 83 | 84 | def jacobians(self, x, u): 85 | """Compute Jacobian of forward dynamics wrt x and u.""" 86 | return self.dfdx(x, u), self.dfdu(x, u) 87 | 88 | 89 | def main(): 90 | # model parameters 91 | mass = 1.0 92 | inertia = np.eye(3) 93 | 94 | # state and input 95 | x = util.zero_state() 96 | u = np.ones(INPUT_DIM) 97 | 98 | # C++-based model which we bind to and load from a shared lib 99 | t = time.time() 100 | cpp_model = CppDynamicsModelWrapper(mass, inertia) 101 | dfdx_cpp, dfdu_cpp = cpp_model.jacobians(x, u) 102 | print(f"Time to load C++ model = {time.time() - t}") 103 | 104 | # jax-based model which is computed just in time 105 | t = time.time() 106 | jax_model = JaxDynamicsModel(mass, inertia) 107 | dfdx_jax, dfdu_jax = jax_model.jacobians(x, u) 108 | print(f"Time to load Jax model = {time.time() - t}") 109 | 110 | acc_cpp = cpp_model.evaluate(x, u) 111 | acc_jax = jax_model.evaluate(x, u) 112 | 113 | # check that both models actually get the same results 114 | assert np.isclose( 115 | acc_cpp, acc_jax 116 | ).all(), "Forward dynamics result not the same between models" 117 | assert np.isclose(dfdx_cpp, dfdx_jax).all(), "dfdx is not the same between models." 118 | assert np.isclose(dfdu_cpp, dfdu_jax).all(), "dfdu is not the same between models." 119 | 120 | # compare runtime evaluation time of Jacobians 121 | n = 100000 122 | cpp_time = timeit.timeit("cpp_model.jacobians(x, u)", number=n, globals=locals()) 123 | jax_time = timeit.timeit("jax_model.jacobians(x, u)", number=n, globals=locals()) 124 | print(f"Time to evaluate C++ model jacobians {n} times = {cpp_time} sec") 125 | print(f"Time to evaluate JAX model jacobians {n} times = {jax_time} sec") 126 | 127 | 128 | if __name__ == "__main__": 129 | main() 130 | -------------------------------------------------------------------------------- /examples/dynamics/scripts/test_rollout_model.py: -------------------------------------------------------------------------------- 1 | import os 2 | from functools import partial 3 | import time 4 | import timeit 5 | 6 | import numpy as np 7 | import jax 8 | import jax.numpy as jnp 9 | 10 | from CppADCodeGenEigenPy import CompiledModel 11 | 12 | import util 13 | 14 | # use 64-bit floating point numbers with jax to match the C++ model 15 | # otherwise, small numerical precision errors can cause model mismatch 16 | jax.config.update("jax_enable_x64", True) 17 | 18 | 19 | SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__)) 20 | LIB_DIR = SCRIPT_DIR + "/../lib" 21 | ROLLOUT_COST_MODEL_NAME = "RolloutCostModel" 22 | 23 | STATE_DIM = 7 + 6 24 | INPUT_DIM = 6 25 | 26 | # NOTE these are currently fixed on the C++ side, and need to be set to the 27 | # same values here 28 | NUM_TIME_STEPS = 10 29 | TIMESTEP = 0.1 30 | 31 | 32 | class CppRolloutCostModelWrapper: 33 | """Wrapper around C++ bindings for DynamicsModel.""" 34 | 35 | def __init__(self, mass, inertia): 36 | lib_path = os.path.join(LIB_DIR, "lib" + ROLLOUT_COST_MODEL_NAME) 37 | self._model = CompiledModel(ROLLOUT_COST_MODEL_NAME, lib_path) 38 | 39 | self.mass = mass 40 | self.inertia = inertia 41 | 42 | def _params(self, xds): 43 | """Create the parameter vector.""" 44 | return np.concatenate([[self.mass], self.inertia.flatten(), xds.flatten()]) 45 | 46 | def _input(self, x0, us): 47 | return np.concatenate((x0, us.flatten())) 48 | 49 | def evaluate(self, x0, us, xds): 50 | """Compute acceleration using forward dynamics.""" 51 | return self._model.evaluate(self._input(x0, us), self._params(xds)) 52 | 53 | def jacobians(self, x0, us, xds): 54 | """Compute derivatives of forward dynamics w.r.t. state x and force input u.""" 55 | J = self._model.jacobian(self._input(x0, us), self._params(xds)) 56 | dfdx0 = J[:, :STATE_DIM] 57 | dfdus = J[:, STATE_DIM:] 58 | return dfdx0, dfdus 59 | 60 | 61 | def orientation_error(q, qd): 62 | """Error between two quaternions.""" 63 | # This is the vector portion of qd.inverse() * q 64 | return qd[3] * q[:3] - q[3] * qd[:3] - jnp.cross(qd[:3], q[:3]) 65 | 66 | 67 | def state_error(x, xd): 68 | """Error between actual state x and desired state xd.""" 69 | r, q, v, ω = util.decompose_state(x) 70 | rd, qd, vd, ωd = util.decompose_state(xd) 71 | 72 | r_err = rd - r 73 | q_err = orientation_error(q, qd) 74 | v_err = vd - v 75 | ω_err = ωd - ω 76 | 77 | return jnp.concatenate((r_err, q_err, v_err, ω_err)) 78 | 79 | 80 | def integrate_state(x0, A, dt): 81 | """Integrate state x0 given acceleration A over timestep dt.""" 82 | r0, q0, v0, ω0 = util.decompose_state(x0) 83 | a, α = A[:3], A[3:] 84 | 85 | r1 = r0 + dt * v0 + 0.5 * dt ** 2 * a 86 | v1 = v0 + dt * a 87 | 88 | ω1 = ω0 + dt * α 89 | 90 | aa = 0.5 * dt * (ω0 + ω1) 91 | angle = jnp.linalg.norm(aa) 92 | axis = aa / angle 93 | 94 | c = jnp.cos(0.5 * angle) 95 | s = jnp.sin(0.5 * angle) 96 | qw = jnp.append(s * axis, c) 97 | q1 = util.quaternion_multiply(qw, q0) 98 | 99 | return jnp.concatenate((r1, q1, v1, ω1)) 100 | 101 | 102 | class JaxRolloutCostModel: 103 | """Equivalent cost model in Python with JAX.""" 104 | 105 | def __init__(self, mass, inertia): 106 | self.mass = mass 107 | self.inertia = inertia 108 | self.inertia_inv = jnp.linalg.inv(inertia) 109 | 110 | self.dfdx0 = jax.jit(jax.jacfwd(self.evaluate, argnums=0)) 111 | self.dfdus = jax.jit(jax.jacfwd(self.evaluate, argnums=1)) 112 | 113 | @partial(jax.jit, static_argnums=(0,)) 114 | def forward_dynamics(self, x, u): 115 | f, τ = u[:3], u[3:] 116 | _, q, v, ω = util.decompose_state(x) 117 | 118 | C_wo = util.quaternion_to_rotation_matrix(q) 119 | Iw = C_wo @ self.inertia @ C_wo.T 120 | Iw_inv = C_wo @ self.inertia_inv @ C_wo.T 121 | 122 | a = f / self.mass 123 | α = Iw_inv @ (τ - jnp.cross(ω, Iw @ ω)) 124 | 125 | return jnp.concatenate((a, α)) 126 | 127 | def evaluate(self, x0, us, xds): 128 | """Compute the cost.""" 129 | 130 | def state_func(x, u): 131 | A = self.forward_dynamics(x, u) 132 | x = integrate_state(x, A, TIMESTEP) 133 | return x, x 134 | 135 | _, xs = jax.lax.scan(state_func, x0, us) 136 | 137 | def cost_func(cost, datum): 138 | x, xd, u = ( 139 | datum[:STATE_DIM], 140 | datum[STATE_DIM : 2 * STATE_DIM], 141 | datum[-INPUT_DIM:], 142 | ) 143 | e = state_error(x, xd) 144 | cost = cost + 0.5 * (e @ e + 0.1 * u @ u) 145 | return cost, datum 146 | 147 | data = jnp.hstack((xs, xds, us)) 148 | cost, _ = jax.lax.scan(cost_func, 0, data) 149 | 150 | return cost 151 | 152 | def jacobians(self, x0, us, xds): 153 | """Compute Jacobians of cost wrt initial state x0 and inputs us.""" 154 | return self.dfdx0(x0, us, xds), self.dfdus(x0, us, xds).flatten() 155 | 156 | 157 | def main(): 158 | np.random.seed(0) 159 | 160 | # model parameters 161 | mass = 1.0 162 | inertia = np.eye(3) 163 | 164 | # initial state 165 | x0 = util.zero_state() 166 | 167 | # force/torque inputs 168 | us = np.random.random((NUM_TIME_STEPS, INPUT_DIM)) 169 | 170 | # desired states 171 | xd = util.zero_state() 172 | xd[:3] = [1, 1, 1] # want body to move position 173 | xds = np.tile(xd, (NUM_TIME_STEPS, 1)) 174 | 175 | # C++-based model which we bind to and load from a shared lib 176 | t = time.time() 177 | cpp_model = CppRolloutCostModelWrapper(mass, inertia) 178 | dfdx0_cpp, dfdus_cpp = cpp_model.jacobians(x0, us, xds) 179 | print(f"Time to load C++ model jacobians = {time.time() - t} sec") 180 | 181 | # jax-based model which is computed just in time 182 | t = time.time() 183 | jax_model = JaxRolloutCostModel(mass, inertia) 184 | dfdx0_jax, dfdus_jax = jax_model.jacobians(x0, us, xds) 185 | print(f"Time to load JAX model jacobians = {time.time() - t} sec") 186 | 187 | cost_cpp = cpp_model.evaluate(x0, us, xds) 188 | cost_jax = jax_model.evaluate(x0, us, xds) 189 | 190 | # check that both models actually get the same results 191 | assert np.isclose(cost_cpp, cost_jax), "Cost is not the same between models." 192 | assert np.isclose( 193 | dfdx0_cpp, dfdx0_jax 194 | ).all(), "dfdx0 is not the same between models." 195 | assert np.isclose( 196 | dfdus_cpp, dfdus_jax 197 | ).all(), "dfdus is not the same between models." 198 | 199 | # compare runtime evaluation time of Jacobians 200 | n = 100000 201 | cpp_time = timeit.timeit( 202 | "cpp_model.jacobians(x0, us, xds)", number=n, globals=locals() 203 | ) 204 | jax_time = timeit.timeit( 205 | "jax_model.jacobians(x0, us, xds)", number=n, globals=locals() 206 | ) 207 | print(f"Time to evaluate C++ model jacobians {n} times = {cpp_time} sec") 208 | print(f"Time to evaluate JAX model jacobians {n} times = {jax_time} sec") 209 | 210 | 211 | if __name__ == "__main__": 212 | main() 213 | -------------------------------------------------------------------------------- /examples/dynamics/scripts/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import jax.numpy as jnp 3 | 4 | 5 | def skew(v): 6 | """Convert 3-dimensional array to skew-symmetric matrix.""" 7 | return jnp.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 8 | 9 | 10 | def quaternion_multiply(q0, q1): 11 | """Hamilton product of two quaternions, stored (x, y, z, w).""" 12 | v0, w0 = q0[:3], q0[3] 13 | v1, w1 = q1[:3], q1[3] 14 | return jnp.append(w0 * v1 + w1 * v0 + jnp.cross(v0, v1), w0 * w1 - v0 @ v1) 15 | 16 | 17 | def quaternion_to_rotation_matrix(q): 18 | """Convert a quaternion to a rotation matrix.""" 19 | # v, w = q[:3], q[3] 20 | # C = (w ** 2 - v @ v) * jnp.eye(3) + 2 * w * skew(v) + 2 * jnp.outer(v, v) 21 | 22 | # This is the algorithm used by Eigen. It actually produces slightly 23 | # different derivative values than the commented-out lines above, so I had 24 | # to use this version to match the Eigen model. 25 | x, y, z, w = q 26 | 27 | tx = 2 * x 28 | ty = 2 * y 29 | tz = 2 * z 30 | twx = tx * w 31 | twy = ty * w 32 | twz = tz * w 33 | txx = tx * x 34 | txy = tx * y 35 | txz = tx * z 36 | tyy = ty * y 37 | tyz = ty * z 38 | tzz = tz * z 39 | 40 | return jnp.array([ 41 | [1 - (tyy + tzz), txy - twz, txz + twy], 42 | [txy + twz, 1 - (txx + tzz), tyz - twx], 43 | [txz - twy, tyz + twx, 1 - (txx + tyy)]]) 44 | 45 | 46 | def decompose_state(x): 47 | """Decompose state into position, orientation, linear and angular velocity.""" 48 | r = x[:3] 49 | q = x[3:7] 50 | v = x[7:10] 51 | ω = x[10:] 52 | return r, q, v, ω 53 | 54 | 55 | def zero_state(): 56 | x = np.zeros(7 + 6) 57 | x[6] = 1 # for quaternion 58 | return x 59 | 60 | -------------------------------------------------------------------------------- /examples/dynamics/src/compile_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "dynamics_model.h" 4 | #include "rollout_model.h" 5 | 6 | namespace ad = CppADCodeGenEigenPy; 7 | 8 | using Scalar = double; 9 | 10 | int main(int argc, char **argv) { 11 | if (argc < 2) { 12 | std::cerr << "Usage: compile_model " << std::endl; 13 | return 1; 14 | } 15 | std::string directory_path = argv[1]; 16 | 17 | DynamicsModel().compile("DynamicsModel", directory_path, 18 | ad::DerivativeOrder::First); 19 | RolloutCostModel().compile("RolloutCostModel", directory_path, 20 | ad::DerivativeOrder::First); 21 | } 22 | -------------------------------------------------------------------------------- /examples/pinocchio/.gitignore: -------------------------------------------------------------------------------- 1 | bin/ 2 | lib/ 3 | cppadcg_sources/ 4 | -------------------------------------------------------------------------------- /examples/pinocchio/Makefile: -------------------------------------------------------------------------------- 1 | CCPP=g++ 2 | SRC_DIR=src 3 | BIN_DIR=bin 4 | LIB_DIR=lib 5 | URDF_DIR=urdf 6 | COMPILER_BIN=$(BIN_DIR)/compile_model 7 | COMPILER_SRC=$(SRC_DIR)/compile_model.cpp 8 | 9 | UR_NAME=ur5 10 | KINOVA_NAME=kinova 11 | URDF_PATH=urdf/kinova.urdf 12 | 13 | INCLUDE_DIRS=-I/usr/local/include/eigen3 -Iinclude 14 | CPP_FLAGS=-std=c++11 15 | PINOCCHIO_FLAGS=$(shell pkg-config --cflags --libs pinocchio) 16 | 17 | # make the compiler for the model 18 | .PHONY: compiler 19 | compiler: 20 | @mkdir -p $(BIN_DIR) 21 | $(CCPP) $(INCLUDE_DIRS) $(CPP_FLAGS) $(COMPILER_SRC) -ldl -o $(COMPILER_BIN) $(PINOCCHIO_FLAGS) 22 | 23 | # use the compiler to generate the robot inverse dynamics models 24 | .PHONY: model 25 | model: 26 | @mkdir -p $(LIB_DIR)/$(UR_NAME) 27 | @mkdir -p $(LIB_DIR)/$(KINOVA_NAME) 28 | ./$(COMPILER_BIN) $(LIB_DIR)/$(UR_NAME) $(URDF_DIR)/$(UR_NAME).urdf 29 | ./$(COMPILER_BIN) $(LIB_DIR)/$(KINOVA_NAME) $(URDF_DIR)/$(KINOVA_NAME).urdf 30 | 31 | # clean up 32 | .PHONY: clean 33 | clean: 34 | @rm -rf $(BIN_DIR) 35 | @rm -rf $(LIB_DIR) 36 | -------------------------------------------------------------------------------- /examples/pinocchio/README.md: -------------------------------------------------------------------------------- 1 | # CppADCodeGenEigenPy Pinocchio Example 2 | 3 | This is an example of using CppADCodeGenEigenPy with the 4 | [Pinocchio](https://github.com/stack-of-tasks/pinocchio) kinematics and 5 | dynamics library. The functionality is very similar to Pinocchio's built-in 6 | [CppADCodeGen support](https://github.com/stack-of-tasks/pinocchio/tree/master/examples/codegen). 7 | 8 | The [InverseDynamicsModel](include/inverse_dynamics_model.h) calls Pinocchio's 9 | recursive Newton-Euler algorithm (RNEA) routine to compute the joint torques 10 | required to achieve a desired joint acceleration given the joint position and 11 | velocity. We compare the result against Pinocchio's built-in analytical 12 | computation of the derivatives of RNEA to ensure correctness. Examples for UR5 13 | and Kinova manipulators are included. 14 | 15 | This particular example is not particularly useful in practice since Pinocchio 16 | provides the derivatives of RNEA already. Of course, one can easily modify the 17 | example to use arbitrary compositions of Pinocchio functions and other 18 | operations as desired. Pinocchio does already have code generation support, but 19 | this provides an alternative approach. Finally, this example may serve as a 20 | blueprint for applying auto-differentiation to other libraries, as long as they 21 | have implemented the scalar templating to support it. 22 | 23 | ## Usage 24 | 25 | Compile the auto-differentiated models: 26 | ``` 27 | # this compiles the program to generate the model 28 | make compiler 29 | 30 | # actually compile the shared libraries, one for each robot 31 | make model 32 | ``` 33 | 34 | Install required Python dependencies (note that Python 3 is expected): 35 | ``` 36 | pip install -r requirements.txt 37 | ``` 38 | 39 | Check that the auto-diff model matches the built-in Pinocchio model: 40 | ``` 41 | python scripts/test_inverse_dynamics_model.py 42 | ``` 43 | The robot being used can be changed by editing the `ROBOT_NAME` variable in the 44 | script. 45 | -------------------------------------------------------------------------------- /examples/pinocchio/include/inverse_dynamics_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // This has to be included before other CppAD-related headers. It defines some 6 | // things required for auto-diff types to work with Pinocchio. 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace ad = CppADCodeGenEigenPy; 16 | 17 | template 18 | struct InverseDynamicsModel : public ad::ADModel { 19 | using typename ad::ADModel::ADScalar; 20 | using typename ad::ADModel::ADVector; 21 | 22 | // Template Pinocchio types for auto-diff. 23 | using Model = 24 | pinocchio::ModelTpl; 25 | using Data = 26 | pinocchio::DataTpl; 27 | 28 | InverseDynamicsModel(const pinocchio::Model& model) 29 | : model_(model.template cast()), 30 | ad::ADModel() { 31 | } 32 | 33 | // Generate the input used when differentiating the function 34 | ADVector input() const override { 35 | ADVector input(model_.nq + 2 * model_.nv); 36 | input << pinocchio::neutral(model_), ADVector::Zero(2 * model_.nv); 37 | return input; 38 | } 39 | 40 | /** 41 | * Inverse dynamics: compute torques that achieve desired acceleration 42 | * given position and velocity. 43 | */ 44 | ADVector function(const ADVector& input) const override { 45 | Data data(model_); 46 | 47 | ADVector q = input.head(model_.nq); 48 | ADVector v = input.segment(model_.nq, model_.nv); 49 | ADVector a = input.tail(model_.nv); 50 | 51 | return pinocchio::rnea(model_, data, q, v, a); 52 | } 53 | 54 | Model model_; 55 | }; 56 | -------------------------------------------------------------------------------- /examples/pinocchio/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.21.3 2 | -------------------------------------------------------------------------------- /examples/pinocchio/scripts/test_inverse_dynamics_model.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import numpy as np 4 | import pinocchio 5 | 6 | from CppADCodeGenEigenPy import CompiledModel 7 | 8 | 9 | SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__)) 10 | ROOT_DIR = os.path.join(SCRIPT_DIR, "..") 11 | 12 | ROBOT_NAME = "kinova" # can be either "kinova" or "ur5" 13 | LIB_DIR = os.path.join(ROOT_DIR, "lib", ROBOT_NAME) 14 | AD_MODEL_NAME = "InverseDynamicsModel" 15 | URDF_PATH = os.path.join(ROOT_DIR, "urdf", ROBOT_NAME + ".urdf") 16 | 17 | 18 | class CppInverseDynamicsModelWrapper: 19 | """Wrapper around C++ bindings for InverseDynamicsModel.""" 20 | 21 | def __init__(self, nq, nv): 22 | lib_path = os.path.join(LIB_DIR, "lib" + AD_MODEL_NAME) 23 | self._model = CompiledModel(AD_MODEL_NAME, lib_path) 24 | self.nq = nq 25 | self.nv = nv 26 | 27 | def evaluate(self, q, v, a): 28 | """Compute torques corresponding to position q, velocity v, acceleration a.""" 29 | inp = np.concatenate((q, v, a)) 30 | return self._model.evaluate(inp) 31 | 32 | def jacobians(self, q, v, a): 33 | """Compute derivatives of forward dynamics w.r.t. state x and force input u.""" 34 | inp = np.concatenate((q, v, a)) 35 | J = self._model.jacobian(inp) 36 | dτdq = J[:, : self.nq] 37 | dτdv = J[:, self.nq : self.nq + self.nv] 38 | dτda = J[:, -self.nv :] 39 | return dτdq, dτdv, dτda 40 | 41 | 42 | def main(): 43 | np.set_printoptions(precision=3, suppress=True) 44 | np.random.seed(0) 45 | 46 | # load pinocchio model 47 | pin_model = pinocchio.buildModelFromUrdf(URDF_PATH) 48 | pin_data = pin_model.createData() 49 | 50 | # determine the joint indices that actually correpond to the robot's 51 | # joints; i.e., the joints which pinocchio computes inverse dynamics for 52 | idx_qs = [] 53 | for joint in pin_model.joints: 54 | if joint.idx_q >= 0: 55 | idx_qs.append(joint.idx_q) 56 | 57 | # load our auto-diffed model 58 | ad_model = CppInverseDynamicsModelWrapper(pin_model.nq, pin_model.nv) 59 | 60 | # random joint position, velocity, acceleration 61 | q = pinocchio.randomConfiguration(pin_model) 62 | v = np.random.random(pin_model.nv) - 0.5 63 | a = np.random.random(pin_model.nv) - 0.5 64 | 65 | # compute inverse dynamics and derivatives using built-in pinocchio 66 | # functions 67 | τ_pin = pinocchio.rnea(pin_model, pin_data, q, v, a) 68 | dτdq_pin, dτdv_pin, dτda_pin = pinocchio.computeRNEADerivatives( 69 | pin_model, pin_data, q, v, a 70 | ) 71 | 72 | # compute inverse dynamics and derivatives using our compiled model 73 | τ_ad = ad_model.evaluate(q, v, a) 74 | dτdq_ad_augmented, dτdv_ad, dτda_ad = ad_model.jacobians(q, v, a) 75 | 76 | # a continuous joint q_i is parameterized using cos(q_i) and sin(q_i) 77 | # (i.e., two entries in the q vector). Below, we detect this case (we 78 | # assume nq == 2 implies a continuous joint), and compute the actual 79 | # derivative dτ/dq_i (like Pinocchio) using the derivatives dτ/d cos(q_i) 80 | # and dτ/d sin(q_i) provided by auto-diff. 81 | dτdq_ad = np.zeros_like(dτdq_pin) 82 | idx = 0 83 | for joint in pin_model.joints: 84 | if joint.idx_q < 0: 85 | continue 86 | 87 | if joint.nq == 1: 88 | # normal revolute joint 89 | dτdq_ad[:, idx] = dτdq_ad_augmented[:, joint.idx_q] 90 | elif joint.nq == 2: 91 | # continuous joint 92 | c = q[joint.idx_q] 93 | s = q[joint.idx_q + 1] 94 | dτdc = dτdq_ad_augmented[:, joint.idx_q] 95 | dτds = dτdq_ad_augmented[:, joint.idx_q + 1] 96 | dτdq_ad[:, idx] = -dτdc * s + dτds * c 97 | else: 98 | raise ValueError("I don't know how to handle a joint with nq > 2.") 99 | idx += 1 100 | 101 | # check that the built-in Pinocchio model and our auto-diff model give 102 | # the same results 103 | assert np.isclose(τ_ad, τ_pin).all(), "τ not the same between models" 104 | assert np.isclose(dτdq_ad, dτdq_pin).all(), "dτdq not the same between models" 105 | assert np.isclose(dτdv_ad, dτdv_pin).all(), "dτdv not the same between models" 106 | assert np.isclose(dτda_ad, dτda_pin).all(), "dτda not the same between models" 107 | 108 | print(f"Auto-diffed and built-in Pinocchio models are equal for {ROBOT_NAME} robot.") 109 | 110 | 111 | if __name__ == "__main__": 112 | main() 113 | -------------------------------------------------------------------------------- /examples/pinocchio/src/compile_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "inverse_dynamics_model.h" 4 | 5 | namespace ad = CppADCodeGenEigenPy; 6 | 7 | int main(int argc, char **argv) { 8 | if (argc < 3) { 9 | std::cerr << "Usage: compile_model " 10 | << std::endl; 11 | return 1; 12 | } 13 | std::string output_dir_path = argv[1]; 14 | std::string urdf_path = argv[2]; 15 | 16 | pinocchio::Model model; 17 | pinocchio::urdf::buildModel(urdf_path, model); 18 | InverseDynamicsModel(model).compile( 19 | "InverseDynamicsModel", output_dir_path, ad::DerivativeOrder::First); 20 | } 21 | -------------------------------------------------------------------------------- /examples/pinocchio/urdf/kinova.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | transmission_interface/SimpleTransmission 134 | 135 | hardware_interface/EffortJointInterface 136 | 137 | 138 | hardware_interface/EffortJointInterface 139 | 160 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | transmission_interface/SimpleTransmission 177 | 178 | hardware_interface/EffortJointInterface 179 | 180 | 181 | hardware_interface/EffortJointInterface 182 | 160 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | transmission_interface/SimpleTransmission 220 | 221 | hardware_interface/EffortJointInterface 222 | 223 | 224 | hardware_interface/EffortJointInterface 225 | 160 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | transmission_interface/SimpleTransmission 263 | 264 | hardware_interface/EffortJointInterface 265 | 266 | 267 | hardware_interface/EffortJointInterface 268 | 160 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | transmission_interface/SimpleTransmission 306 | 307 | hardware_interface/EffortJointInterface 308 | 309 | 310 | hardware_interface/EffortJointInterface 311 | 160 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | transmission_interface/SimpleTransmission 349 | 350 | hardware_interface/EffortJointInterface 351 | 352 | 353 | hardware_interface/EffortJointInterface 354 | 160 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | transmission_interface/SimpleTransmission 395 | 396 | hardware_interface/EffortJointInterface 397 | 398 | 399 | hardware_interface/EffortJointInterface 400 | 1 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | transmission_interface/SimpleTransmission 432 | 433 | hardware_interface/EffortJointInterface 434 | 435 | 436 | hardware_interface/EffortJointInterface 437 | 1 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | transmission_interface/SimpleTransmission 469 | 470 | hardware_interface/EffortJointInterface 471 | 472 | 473 | hardware_interface/EffortJointInterface 474 | 1 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | transmission_interface/SimpleTransmission 506 | 507 | hardware_interface/EffortJointInterface 508 | 509 | 510 | hardware_interface/EffortJointInterface 511 | 1 512 | 513 | 514 | 515 | -------------------------------------------------------------------------------- /examples/pinocchio/urdf/ur5.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | transmission_interface/SimpleTransmission 244 | 245 | hardware_interface/PositionJointInterface 246 | 247 | 248 | 1 249 | 250 | 251 | 252 | transmission_interface/SimpleTransmission 253 | 254 | hardware_interface/PositionJointInterface 255 | 256 | 257 | 1 258 | 259 | 260 | 261 | transmission_interface/SimpleTransmission 262 | 263 | hardware_interface/PositionJointInterface 264 | 265 | 266 | 1 267 | 268 | 269 | 270 | transmission_interface/SimpleTransmission 271 | 272 | hardware_interface/PositionJointInterface 273 | 274 | 275 | 1 276 | 277 | 278 | 279 | transmission_interface/SimpleTransmission 280 | 281 | hardware_interface/PositionJointInterface 282 | 283 | 284 | 1 285 | 286 | 287 | 288 | transmission_interface/SimpleTransmission 289 | 290 | hardware_interface/PositionJointInterface 291 | 292 | 293 | 1 294 | 295 | 296 | 297 | true 298 | 299 | 300 | true 301 | 302 | 303 | true 304 | 305 | 306 | true 307 | 308 | 309 | true 310 | 311 | 312 | true 313 | 314 | 315 | true 316 | 317 | 318 | 319 | 320 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | -------------------------------------------------------------------------------- /examples/simple/.gitignore: -------------------------------------------------------------------------------- 1 | make_model 2 | *.so 3 | -------------------------------------------------------------------------------- /examples/simple/Makefile: -------------------------------------------------------------------------------- 1 | EIGEN_INCLUDE_DIRS=-I/usr/include/eigen3 -I/usr/local/include/eigen3 2 | 3 | .PHONY: all 4 | all: 5 | g++ -std=c++11 $(EIGEN_INCLUDE_DIRS) model.cpp -ldl -o make_model 6 | ./make_model 7 | -------------------------------------------------------------------------------- /examples/simple/README.md: -------------------------------------------------------------------------------- 1 | # CppADCodeGenEigenPy Simple Example 2 | 3 | This is a basic example of CppADCodeGenEigenPy usage where we 4 | auto-differentiate a weight sum-of-squares model with respect to the model 5 | inputs. The model is defined in `model.cpp`. 6 | 7 | ## Usage 8 | 9 | 1. Compile and build the model by running `make`. 10 | 2. Try it out from a Python script: `python load_model.py`. 11 | -------------------------------------------------------------------------------- /examples/simple/load_model.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from CppADCodeGenEigenPy import CompiledModel 3 | 4 | # note that the .so file extension not included on the second argument 5 | model = CompiledModel("ExampleModel", "libExampleModel") 6 | 7 | inputs = np.array([1, 2, 3]) 8 | params = np.ones(3) 9 | 10 | # compute model output and first derivative 11 | output = model.evaluate(inputs, params) 12 | jacobian = model.jacobian(inputs, params) 13 | 14 | print(f"output = {output}") 15 | print(f"jacobian = {jacobian}") 16 | -------------------------------------------------------------------------------- /examples/simple/model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace ad = CppADCodeGenEigenPy; 5 | 6 | // Our custom model extends the ad::ADModel class 7 | template 8 | struct ExampleModel : public ad::ADModel { 9 | using typename ad::ADModel::ADVector; 10 | 11 | // Generate the input used when differentiating the function 12 | ADVector input() const override { return ADVector::Ones(3); } 13 | 14 | // Generate parameters used when differentiating the function 15 | // This can be skipped if the function takes no parameters 16 | ADVector parameters() const override { return ADVector::Ones(3); } 17 | 18 | // Implement a weighted sum-of-squares function. 19 | ADVector function(const ADVector& input, 20 | const ADVector& parameters) const override { 21 | ADVector output(1); 22 | for (int i = 0; i < 3; ++i) { 23 | output(0) += 0.5 * parameters(i) * input(i) * input(i); 24 | } 25 | return output; 26 | } 27 | }; 28 | 29 | int main() { 30 | // Compile model named ExampleModel and save in the current directory; the 31 | // model is saved as a shared object file named libExampleModel.so 32 | ExampleModel().compile("ExampleModel", ".", 33 | ad::DerivativeOrder::First); 34 | } 35 | -------------------------------------------------------------------------------- /include/CppADCodeGenEigenPy/ADModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace CppADCodeGenEigenPy { 11 | 12 | /** Derivative order */ 13 | enum class DerivativeOrder { Zero, First, Second }; 14 | 15 | /** Abstract base class for a function to be auto-differentiated and then 16 | * compiled. 17 | * 18 | * This class should be subclassed to define the function and its inputs and 19 | * (optional) parameters. It can then be compiled to a dynamic library and 20 | * loaded for use with the `Model` class. 21 | * 22 | * @tparam Scalar The scalar type to use. Typically float or double. 23 | */ 24 | template 25 | class ADModel { 26 | public: 27 | using ADScalarBase = CppAD::cg::CG; 28 | 29 | /** Auto-diff scalar type. */ 30 | using ADScalar = CppAD::AD; 31 | 32 | /** Auto-diff dynamic vector type. */ 33 | using ADVector = Eigen::Matrix; 34 | 35 | /** Auto-diff dynamic matrix type. */ 36 | // Needs to be row major to interface with numpy 37 | using ADMatrix = Eigen::Matrix; 39 | 40 | /** Constructor. */ 41 | ADModel() {} 42 | 43 | /** Compile the library into a dynamic library. 44 | * 45 | * @param[in] model_name Name of the compiled model. 46 | * @param[in] directory_path Path of directory where model library should 47 | * go. The direcotory must exist; it will not be 48 | * created automatically. 49 | * @param[in] verbose Print additional information. 50 | * @param[in] save_sources Save the C files generated by CppADCodeGen. 51 | * @param[in] compile_flags Flags to pass to the compiler to compile the 52 | * library. 53 | * 54 | * @returns The compiled model. 55 | */ 56 | CompiledModel compile( 57 | const std::string& model_name, const std::string& directory_path, 58 | DerivativeOrder order = DerivativeOrder::Second, bool verbose = false, 59 | bool save_sources = false, 60 | std::vector compile_flags = { 61 | "-O3", "-march=native", "-mtune=native", "-ffast-math"}) const; 62 | 63 | protected: 64 | /** Defines this model's (parameterless) function. 65 | * 66 | * This should be overridden by derived classes implementing a model with 67 | * no parameters. Only one of the overloads of `function` should be 68 | * overridden by a single derived class. 69 | * 70 | * @param[in] x The input to the function. 71 | * 72 | * @returns The function output. 73 | */ 74 | virtual ADVector function(const ADVector& x) const; 75 | 76 | /** Defines this model's (parameterized) function. 77 | * 78 | * This should be overridden by derived classes implementing a model with 79 | * parameters. Parameters differ from inputs only in that derivatives are 80 | * not taken with respect to parameters. Only one of the overloads of 81 | * `function` should be overridden by a single derived class. 82 | * 83 | * @param[in] x The input to the function. 84 | * @param[in] p The function parameters. 85 | * 86 | * @returns The function output. 87 | */ 88 | virtual ADVector function(const ADVector& x, const ADVector& p) const; 89 | 90 | /** Generates the input that should be used when recording the operation 91 | * sequence for auto-differentiation. Also defines the shape of the input. 92 | * Must be defined by the derived class. 93 | * 94 | * When in doubt, a good choice is often to generate a vector of ones. A 95 | * vector of zeros is often more liable to produce divisions by zero when 96 | * differentiating, but it depends on the particular function. 97 | * 98 | * @returns The input vector to use for auto-differentiation. 99 | */ 100 | virtual ADVector input() const = 0; 101 | 102 | /** Generates the parameter vector that should be used when recording the 103 | * operation sequence for auto-differentiation. Also defines the shape of 104 | * the parameter vector. Need only be overridden by the derived class if 105 | * the modelled function takes parameters. 106 | * 107 | * @returns The parameter vector to use for auto-differentiation 108 | */ 109 | virtual ADVector parameters() const; 110 | }; // class ADModel 111 | 112 | #include "impl/ADModel.tpp" 113 | 114 | } // namespace CppADCodeGenEigenPy 115 | -------------------------------------------------------------------------------- /include/CppADCodeGenEigenPy/CompiledModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace CppADCodeGenEigenPy { 10 | 11 | /** A CompiledModel wraps a dynamic library that has been compiled from derived 12 | * class 13 | * of ADModel. It provides methods for evaluating the function and available 14 | * derivatives (Jacobian, Hessian). 15 | * 16 | * @tparam Scalar The scalar type to use. Typically float or double. 17 | */ 18 | template 19 | class CompiledModel { 20 | public: 21 | /** Dynamic vector type. */ 22 | using Vector = Eigen::Matrix; 23 | 24 | /** Dynamic matrix type. */ 25 | using Matrix = 26 | Eigen::Matrix; 27 | 28 | /** Constructor. 29 | * 30 | * @param[in] model_name The name of the model being loaded from the 31 | * dynamic library. 32 | * @param[in] library_generic_path The full path to the dynamic library, 33 | * without the file extension. 34 | */ 35 | CompiledModel(const std::string& model_name, 36 | const std::string& library_generic_path); 37 | 38 | // ~CompiledModel() = default; 39 | 40 | /** Evaluate the function. This overload should be called if the modelled 41 | * function has no parameters. 42 | * 43 | * @param[in] input The input at which to evaluate the function. 44 | * 45 | * @throws std::runtime_error if the provided input size does not match 46 | * that of the model. 47 | * 48 | * @returns The function output. 49 | */ 50 | Vector evaluate(const Eigen::Ref& input) const; 51 | 52 | /** Evaluate the function. This overload should be called if the modelled 53 | * function has parameters. 54 | * 55 | * @param[in] input The input at which to evaluate the function. 56 | * @param[in] parameters The parameters for the function. 57 | * 58 | * @throws std::runtime_error if the combined size of the provided input 59 | * and parameters does not match the input size of the model. 60 | * 61 | * @returns The function output. 62 | */ 63 | Vector evaluate(const Eigen::Ref& input, 64 | const Eigen::Ref& parameters) const; 65 | 66 | /** Compute the function's Jacobian (first-order derivative). This overload 67 | * should be called if the modelled function has no parameters. 68 | * 69 | * @param[in] input The input at which to evaluate the Jacobian. 70 | * 71 | * @throws std::runtime_error if the provided input size does not match 72 | * that of the model. 73 | * @throws std::runtime_error if the order of the model is not at least 74 | * one. 75 | * 76 | * @returns The Jacobian matrix. 77 | */ 78 | Matrix jacobian(const Eigen::Ref& input) const; 79 | 80 | /** Compute the function's Jacobian (first-order derivative). This overload 81 | * should be called if the modelled function has parameters. 82 | * 83 | * @param[in] input The input at which to evaluate the Jacobian. 84 | * @param[in] parameters The parameters for the function. 85 | * 86 | * @throws std::runtime_error if the combined size of the provided input 87 | * and parameters does not match the input size of the model. 88 | * @throws std::runtime_error if the order of the model is not at least 89 | * one. 90 | * 91 | * @returns The Jacobian matrix. 92 | */ 93 | Matrix jacobian(const Eigen::Ref& input, 94 | const Eigen::Ref& parameters) const; 95 | 96 | /** Compute the function's Hessian (second-order derivative) for a given 97 | * output dimension. This overload should be called if the modelled 98 | * function has no parameters. 99 | * 100 | * @param[in] input The input at which to evaluate the Hessian. 101 | * @param[in] output_dim The output dimension for which to evaluate the 102 | * Hessian. 103 | * 104 | * @throws std::runtime_error if the provided input size does not match 105 | * that of the model. 106 | * @throws std::runtime_error if the order of the model is not at least 107 | * two. 108 | * 109 | * @returns The Hessian matrix. 110 | */ 111 | Matrix hessian(const Eigen::Ref& input, 112 | size_t output_dim = 0) const; 113 | 114 | /** Compute the function's Hessian (second-order derivative) for a given 115 | * output dimension. This overload should be called if the modelled 116 | * function has parameters. 117 | * 118 | * @param[in] input The input at which to evaluate the Hessian. 119 | * @param[in] parameters The parameters for the function. 120 | * @param[in] output_dim The output dimension for which to evaluate the 121 | * Hessian. 122 | * 123 | * @throws std::runtime_error if the combined size of the provided input 124 | * and parameters does not match the input size of the model. 125 | * @throws std::runtime_error if the order of the model is not at least 126 | * two. 127 | * 128 | * @returns The Hessian matrix. 129 | */ 130 | Matrix hessian(const Eigen::Ref& input, 131 | const Eigen::Ref& parameters, 132 | size_t output_dim = 0) const; 133 | 134 | /** Get the input size of the model. Note that this includes both normal 135 | * inputs and parameters. 136 | * 137 | * @returns The combined size of the model input and parameters. 138 | */ 139 | size_t get_input_size() const; 140 | 141 | /** Get the output size of the model. 142 | * 143 | * @returns The size of the model output. 144 | */ 145 | size_t get_output_size() const; 146 | 147 | private: 148 | std::unique_ptr> lib_; 149 | std::unique_ptr> model_; 150 | 151 | size_t input_size_; 152 | size_t output_size_; 153 | 154 | // Error if input size is wrong. If it is too small,the user may have 155 | // meant to pass parameters as well. 156 | void check_input_size(size_t size) const; 157 | 158 | // Error if input + parameters vector is too large, which may mean the 159 | // user shouldn't have passed parameters 160 | void check_input_size_with_params(size_t input_size, 161 | size_t param_size) const; 162 | }; // class CompiledModel 163 | 164 | #include "impl/CompiledModel.tpp" 165 | 166 | } // namespace CppADCodeGenEigenPy 167 | -------------------------------------------------------------------------------- /include/CppADCodeGenEigenPy/Util.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | // TODO comment these 7 | 8 | inline std::string get_library_generic_path(const std::string& model_name, 9 | const std::string& directory_path) { 10 | return directory_path + "/lib" + model_name; 11 | } 12 | 13 | inline std::string get_library_real_path(const std::string& model_name, 14 | const std::string& directory_path) { 15 | std::string ext = CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION; 16 | return get_library_generic_path(model_name, directory_path) + ext; 17 | } 18 | 19 | inline void error_handler(bool known, int line, const char* file, 20 | const char* exp, const char* msg) { 21 | throw std::runtime_error(msg); 22 | } 23 | -------------------------------------------------------------------------------- /include/CppADCodeGenEigenPy/impl/ADModel.tpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | template 4 | CompiledModel ADModel::compile( 5 | const std::string& model_name, const std::string& directory_path, 6 | DerivativeOrder order, bool verbose, bool save_sources, 7 | std::vector compile_flags) const { 8 | ADVector x = input(); 9 | ADVector p = parameters(); 10 | ADVector xp(x.rows() + p.rows()); 11 | xp << x, p; 12 | 13 | CppAD::Independent(xp); 14 | 15 | // Apply the model function to get output 16 | ADVector y = function(xp.head(x.rows()), xp.tail(p.rows())); 17 | 18 | // Record the relationship for AD 19 | // It is more efficient to do: 20 | // ADFun f; 21 | // f.Dependent(x, y); 22 | // f.optimize(); 23 | // rather than 24 | // ADFun f(x, y); 25 | // f.optimize(); 26 | // see 27 | CppAD::ADFun ad_func; 28 | ad_func.Dependent(xp, y); 29 | 30 | // Optimize the operation sequence 31 | ad_func.optimize(); 32 | 33 | // Generate source code 34 | // TODO support sparse Jacobian/Hessian 35 | CppAD::cg::ModelCSourceGen source_gen(ad_func, model_name); 36 | if (order >= DerivativeOrder::First) { 37 | source_gen.setCreateJacobian(true); 38 | } 39 | if (order >= DerivativeOrder::Second) { 40 | source_gen.setCreateHessian(true); 41 | } 42 | 43 | const std::string lib_generic_path = 44 | get_library_generic_path(model_name, directory_path); 45 | CppAD::cg::ModelLibraryCSourceGen lib_source_gen(source_gen); 46 | CppAD::cg::GccCompiler compiler; 47 | CppAD::cg::DynamicModelLibraryProcessor lib_processor( 48 | lib_source_gen, lib_generic_path); 49 | 50 | if (save_sources) { 51 | CppAD::cg::SaveFilesModelLibraryProcessor lib_source_saver( 52 | lib_source_gen); 53 | lib_source_saver.saveSources(); 54 | } 55 | 56 | compiler.setCompileLibFlags(compile_flags); 57 | compiler.addCompileLibFlag("-shared"); 58 | compiler.addCompileLibFlag("-rdynamic"); 59 | 60 | // Compile the library 61 | std::unique_ptr> lib = 62 | lib_processor.createDynamicLibrary(compiler); 63 | if (verbose) { 64 | std::cout << "Compiled library for model " << model_name << " to " 65 | << get_library_real_path(model_name, directory_path) 66 | << std::endl; 67 | } 68 | return CompiledModel(model_name, lib_generic_path); 69 | } 70 | 71 | template 72 | typename ADModel::ADVector ADModel::function( 73 | const ADVector& x) const { 74 | // Default is a single 0. 75 | return ADVector::Zero(1); 76 | } 77 | 78 | template 79 | typename ADModel::ADVector ADModel::function( 80 | const ADVector& x, const ADVector& p) const { 81 | // We only want user to have to override one of the "function" methods, 82 | // which means neither can be pure virtual. This is one downside: the 83 | // compiler will let you compile a model with a default function value. 84 | // 85 | // This overload will always be called by the other code in the class, 86 | // so either: 87 | // * this one is overridden, and this implementation does not matter 88 | // * the first is overridden, which is called by this 89 | return function(x); 90 | } 91 | 92 | template 93 | typename ADModel::ADVector ADModel::parameters() const { 94 | // Default is an empty parameter vector. 95 | return ADVector(0); 96 | } 97 | -------------------------------------------------------------------------------- /include/CppADCodeGenEigenPy/impl/CompiledModel.tpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | template 4 | CompiledModel::CompiledModel(const std::string& model_name, 5 | const std::string& library_generic_path) { 6 | // Replace CppAD error handler so that error is thrown if dynamic library 7 | // cannot be loaded for some reason. See 8 | // https://coin-or.github.io/CppAD/doc/error_handler.cpp.htm 9 | CppAD::ErrorHandler handler(error_handler); 10 | 11 | lib_.reset(new CppAD::cg::LinuxDynamicLib( 12 | library_generic_path + 13 | CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION)); 14 | model_ = lib_->model(model_name); 15 | input_size_ = model_->Domain(); 16 | output_size_ = model_->Range(); 17 | } 18 | 19 | template 20 | typename CompiledModel::Vector CompiledModel::evaluate( 21 | const Eigen::Ref& input) const { 22 | check_input_size(input.size()); 23 | return model_->template ForwardZero(input); 24 | } 25 | 26 | template 27 | typename CompiledModel::Vector CompiledModel::evaluate( 28 | const Eigen::Ref& input, 29 | const Eigen::Ref& parameters) const { 30 | check_input_size_with_params(input.size(), parameters.size()); 31 | 32 | Vector xp(input.size() + parameters.size()); 33 | xp << input, parameters; 34 | return evaluate(xp); 35 | } 36 | 37 | template 38 | typename CompiledModel::Matrix CompiledModel::jacobian( 39 | const Eigen::Ref& input) const { 40 | if (!model_->isJacobianAvailable()) { 41 | throw std::runtime_error( 42 | "Jacobian is not available: compiled model must be at least " 43 | "first-order."); 44 | } 45 | check_input_size(input.size()); 46 | 47 | assert(input.rows() == input_size_); 48 | Vector J_vec = model_->template Jacobian(input); 49 | Eigen::Map J(J_vec.data(), output_size_, input_size_); 50 | assert(J.allFinite()); 51 | return J; 52 | } 53 | 54 | template 55 | typename CompiledModel::Matrix CompiledModel::jacobian( 56 | const Eigen::Ref& input, 57 | const Eigen::Ref& parameters) const { 58 | check_input_size_with_params(input.size(), parameters.size()); 59 | 60 | Vector xp(input.size() + parameters.size()); 61 | xp << input, parameters; 62 | return jacobian(xp).leftCols(input.rows()); 63 | } 64 | 65 | template 66 | typename CompiledModel::Matrix CompiledModel::hessian( 67 | const Eigen::Ref& input, size_t output_dim) const { 68 | if (!model_->isHessianAvailable()) { 69 | throw std::runtime_error( 70 | "Hessian is not available: compiled model must be " 71 | "second-order."); 72 | } 73 | if (output_dim >= output_size_) { 74 | throw std::runtime_error("Specified output dimension for Hessian is " + 75 | std::to_string(output_dim) + 76 | ", but model has only " + 77 | std::to_string(output_size_) + " outputs."); 78 | } 79 | check_input_size(input.size()); 80 | 81 | // Need to use the overload of the Hessian function that accepts a weight 82 | // vector w. Otherwise, CppADCodeGen creates a w vector but does not 83 | // initialize it to zero, which can cause errors. 84 | Vector w = Vector::Zero(output_size_); 85 | w(output_dim) = 1.0; 86 | Vector H_vec = model_->template Hessian(input, w); 87 | Eigen::Map H(H_vec.data(), input_size_, input_size_); 88 | assert(H.allFinite()); 89 | return H; 90 | } 91 | 92 | template 93 | typename CompiledModel::Matrix CompiledModel::hessian( 94 | const Eigen::Ref& input, 95 | const Eigen::Ref& parameters, size_t output_dim) const { 96 | check_input_size_with_params(input.size(), parameters.size()); 97 | 98 | Vector xp(input.size() + parameters.size()); 99 | xp << input, parameters; 100 | return hessian(xp, output_dim).topLeftCorner(input.rows(), input.rows()); 101 | } 102 | 103 | template 104 | size_t CompiledModel::get_input_size() const { 105 | return input_size_; 106 | } 107 | 108 | template 109 | size_t CompiledModel::get_output_size() const { 110 | return output_size_; 111 | } 112 | 113 | template 114 | void CompiledModel::check_input_size(size_t size) const { 115 | if (size < input_size_) { 116 | throw std::runtime_error( 117 | "Model domain is " + std::to_string(input_size_) + 118 | ", but input is of size " + std::to_string(size) + 119 | ". Did you mean to pass parameters, too?"); 120 | } else if (size > input_size_) { 121 | throw std::runtime_error( 122 | "Model domain is " + std::to_string(input_size_) + 123 | ", but input is of size " + std::to_string(size)); 124 | } 125 | } 126 | 127 | template 128 | void CompiledModel::check_input_size_with_params( 129 | size_t input_size, size_t param_size) const { 130 | size_t total_size = input_size + param_size; 131 | if (total_size > input_size_) { 132 | throw std::runtime_error( 133 | "Input size is " + std::to_string(input_size) + 134 | " and parameter size is " + std::to_string(param_size) + 135 | ". The total is " + std::to_string(total_size) + 136 | ", which larger than the model domain " + 137 | std::to_string(input_size_) + 138 | ". Maybe you meant not to pass parameters?"); 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=42", "wheel", "pybind11>=2.7.0"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [tool.pytest.ini_options] 6 | testpaths = ["tests/python_tests"] 7 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from pybind11.setup_helpers import Pybind11Extension, build_ext 5 | 6 | 7 | with open("README.md") as f: 8 | long_description = f.read() 9 | 10 | with open("VERSION") as f: 11 | version = f.read().strip() 12 | 13 | 14 | ext_modules = [ 15 | # the eigen3 include directories below may be required for the compiler to 16 | # find Eigen 17 | Pybind11Extension( 18 | "CppADCodeGenEigenPy", 19 | ["src/bindings.cpp"], 20 | include_dirs=["include", "/usr/include/eigen3", "/usr/local/include/eigen3"], 21 | ), 22 | ] 23 | 24 | setup( 25 | name="CppADCodeGenEigenPy", 26 | version=version, 27 | description="CppADCodeGen with an Eigen interface and Python bindings.", 28 | long_description=long_description, 29 | long_description_content_type="text/markdown", 30 | author="Adam Heins", 31 | author_email="mail@adamheins.com", 32 | install_requires=["numpy"], 33 | extras_require={"test": "pytest"}, 34 | ext_modules=ext_modules, 35 | cmdclass={"build_ext": build_ext}, 36 | license="MIT", 37 | zip_safe=False, 38 | ) 39 | -------------------------------------------------------------------------------- /src/bindings.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace py = pybind11; 9 | namespace ad = CppADCodeGenEigenPy; 10 | 11 | PYBIND11_MODULE(CppADCodeGenEigenPy, m) { 12 | using Scalar = double; 13 | using Vector = ad::CompiledModel::Vector; 14 | using Matrix = ad::CompiledModel::Matrix; 15 | 16 | m.doc() = "Bindings for code auto-differentiated using CppADCodeGen."; 17 | 18 | // TODO I've hardcoded double into this for now---not sure if I can 19 | // actually get away from this... 20 | py::class_>(m, "CompiledModel") 21 | .def(py::init()) 22 | .def("evaluate", static_cast::*)( 23 | const Eigen::Ref&) const>( 24 | &ad::CompiledModel::evaluate), 25 | "Evaluate function with no parameters.") 26 | .def("evaluate", static_cast::*)( 27 | const Eigen::Ref&, 28 | const Eigen::Ref&) const>( 29 | &ad::CompiledModel::evaluate), 30 | "Evaluate function with parameters.") 31 | .def("jacobian", static_cast::*)( 32 | const Eigen::Ref&) const>( 33 | &ad::CompiledModel::jacobian), 34 | "Evaluate Jacobian with no parameters.") 35 | .def("jacobian", static_cast::*)( 36 | const Eigen::Ref&, 37 | const Eigen::Ref&) const>( 38 | &ad::CompiledModel::jacobian), 39 | "Evaluate Jacobian with parameters.") 40 | .def("hessian", static_cast::*)( 41 | const Eigen::Ref&, size_t) const>( 42 | &ad::CompiledModel::hessian), 43 | "Evaluate Hessian with no parameters.") 44 | .def("hessian", static_cast::*)( 45 | const Eigen::Ref&, 46 | const Eigen::Ref&, size_t) const>( 47 | &ad::CompiledModel::hessian), 48 | "Evaluate Hessian with parameters.") 49 | .def_property_readonly("input_size", 50 | &ad::CompiledModel::get_input_size) 51 | .def_property_readonly("output_size", 52 | &ad::CompiledModel::get_output_size); 53 | } 54 | -------------------------------------------------------------------------------- /tests/cpp_tests/BasicModelTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "testing/models/BasicTestModel.h" 11 | 12 | namespace CppADCodeGenEigenPy { 13 | namespace BasicModelTest { 14 | 15 | class BasicTestModelFixture : public ::testing::Test { 16 | protected: 17 | using Vector = CompiledModel::Vector; 18 | using Matrix = CompiledModel::Matrix; 19 | 20 | static void SetUpTestSuite() { 21 | // Compile and load our model 22 | boost::filesystem::create_directories(DIRECTORY_PATH); 23 | ad_model_ptr_.reset(new BasicTestModel()); 24 | ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH, 25 | DerivativeOrder::Second); 26 | compiled_model_ptr_.reset( 27 | new CompiledModel(MODEL_NAME, LIB_GENERIC_PATH)); 28 | } 29 | 30 | static void TearDownTestSuite() { 31 | // Delete the compiled shared object. 32 | boost::filesystem::remove_all(DIRECTORY_PATH); 33 | } 34 | 35 | static std::unique_ptr> ad_model_ptr_; 36 | static std::unique_ptr> compiled_model_ptr_; 37 | }; 38 | 39 | std::unique_ptr> BasicTestModelFixture::ad_model_ptr_ = nullptr; 40 | std::unique_ptr> 41 | BasicTestModelFixture::compiled_model_ptr_ = nullptr; 42 | 43 | TEST_F(BasicTestModelFixture, CreatesSharedLib) { 44 | EXPECT_TRUE(boost::filesystem::exists(LIB_REAL_PATH)) 45 | << "Shared library file " << LIB_REAL_PATH << " not found."; 46 | } 47 | 48 | TEST_F(BasicTestModelFixture, Dimensions) { 49 | // test that the model shape is correct 50 | EXPECT_EQ(compiled_model_ptr_->get_input_size(), NUM_INPUT) 51 | << "Input size is incorrect."; 52 | EXPECT_EQ(compiled_model_ptr_->get_output_size(), NUM_OUTPUT) 53 | << "Output size is incorrect."; 54 | 55 | // generate an input with a size too large 56 | Vector input = Vector::Ones(NUM_INPUT + 1); 57 | 58 | EXPECT_THROW(compiled_model_ptr_->evaluate(input), std::runtime_error) 59 | << "Evaluate with input of wrong size did not throw."; 60 | EXPECT_THROW(compiled_model_ptr_->jacobian(input), std::runtime_error) 61 | << "Jacobian with input of wrong size did not throw."; 62 | EXPECT_THROW(compiled_model_ptr_->hessian(input, 0), std::runtime_error) 63 | << "Hessian with input of wrong size did not throw."; 64 | } 65 | 66 | TEST_F(BasicTestModelFixture, Evaluation) { 67 | Vector input = Vector::Ones(NUM_INPUT); 68 | 69 | Vector output_expected = evaluate(input); 70 | Vector output_actual = compiled_model_ptr_->evaluate(input); 71 | EXPECT_TRUE(output_actual.isApprox(output_expected)) 72 | << "Function evaluation is incorrect."; 73 | } 74 | 75 | TEST_F(BasicTestModelFixture, Jacobian) { 76 | Vector input = Vector::Ones(NUM_INPUT); 77 | 78 | Matrix J_expected(NUM_OUTPUT, NUM_INPUT); 79 | J_expected.setZero(); 80 | J_expected.diagonal() << 2, 2, 2; 81 | Matrix J_actual = compiled_model_ptr_->jacobian(input); 82 | 83 | EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect."; 84 | } 85 | 86 | TEST_F(BasicTestModelFixture, Hessian) { 87 | Vector input = Vector::Ones(NUM_INPUT); 88 | 89 | // Hessian of all output dimensions should be zero 90 | Matrix H_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT); 91 | Matrix H0_actual = compiled_model_ptr_->hessian(input, 0); 92 | Matrix H1_actual = compiled_model_ptr_->hessian(input, 1); 93 | Matrix H2_actual = compiled_model_ptr_->hessian(input, 2); 94 | 95 | EXPECT_TRUE(H0_actual.isApprox(H_expected)) 96 | << "Hessian for dim 0 is incorrect."; 97 | EXPECT_TRUE(H1_actual.isApprox(H_expected)) 98 | << "Hessian for dim 1 is incorrect."; 99 | EXPECT_TRUE(H2_actual.isApprox(H_expected)) 100 | << "Hessian for dim 2 is incorrect."; 101 | 102 | EXPECT_THROW(compiled_model_ptr_->hessian(input, NUM_OUTPUT), 103 | std::runtime_error) 104 | << "Hessian with too-large output_dim did not throw."; 105 | } 106 | 107 | } // namespace BasicModelTest 108 | } // namespace CppADCodeGenEigenPy 109 | -------------------------------------------------------------------------------- /tests/cpp_tests/LowOrderModelTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "testing/models/BasicTestModel.h" 10 | 11 | namespace CppADCodeGenEigenPy { 12 | namespace LowOrderModelTest { 13 | 14 | using namespace BasicModelTest; 15 | 16 | class LowOrderTestModelFixture : public ::testing::Test { 17 | protected: 18 | using Vector = CompiledModel::Vector; 19 | using Matrix = CompiledModel::Matrix; 20 | 21 | static void SetUpTestSuite() { 22 | // Here we just reuse the basic model, but only compile with 23 | // zero-order. 24 | boost::filesystem::create_directories(DIRECTORY_PATH); 25 | ad_model_ptr_.reset(new BasicTestModel()); 26 | ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH, 27 | DerivativeOrder::Zero); 28 | compiled_model_ptr_.reset( 29 | new CompiledModel(MODEL_NAME, LIB_GENERIC_PATH)); 30 | } 31 | 32 | static void TearDownTestSuite() { 33 | // Delete the compiled shared object. 34 | boost::filesystem::remove_all(DIRECTORY_PATH); 35 | } 36 | 37 | static std::unique_ptr> ad_model_ptr_; 38 | static std::unique_ptr> compiled_model_ptr_; 39 | }; 40 | 41 | std::unique_ptr> LowOrderTestModelFixture::ad_model_ptr_ = 42 | nullptr; 43 | std::unique_ptr> 44 | LowOrderTestModelFixture::compiled_model_ptr_ = nullptr; 45 | 46 | TEST_F(LowOrderTestModelFixture, ThrowsOnJacobianHessian) { 47 | Vector input = Vector::Ones(NUM_INPUT); 48 | 49 | EXPECT_NO_THROW(compiled_model_ptr_->evaluate(input)) << "Evaluate threw."; 50 | EXPECT_THROW(compiled_model_ptr_->jacobian(input), std::runtime_error) 51 | << "Jacobian did not throw."; 52 | EXPECT_THROW(compiled_model_ptr_->hessian(input, 0), std::runtime_error) 53 | << "Hessian did not throw."; 54 | } 55 | 56 | } // namespace LowOrderModelTest 57 | } // namespace CppADCodeGenEigenPy 58 | -------------------------------------------------------------------------------- /tests/cpp_tests/MathFunctionsModelTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "testing/models/MathFunctionsTestModel.h" 11 | 12 | namespace CppADCodeGenEigenPy { 13 | namespace MathFunctionsModelTest { 14 | 15 | class MathFunctionsTestModelFixture : public ::testing::Test { 16 | protected: 17 | using Vector = CompiledModel::Vector; 18 | using Matrix = CompiledModel::Matrix; 19 | 20 | static void SetUpTestSuite() { 21 | // Compile and load our model 22 | boost::filesystem::create_directories(DIRECTORY_PATH); 23 | ad_model_ptr_.reset(new MathFunctionsTestModel()); 24 | ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH, 25 | DerivativeOrder::Second); 26 | compiled_model_ptr_.reset( 27 | new CompiledModel(MODEL_NAME, LIB_GENERIC_PATH)); 28 | } 29 | 30 | static void TearDownTestSuite() { 31 | // Delete the compiled shared object. 32 | boost::filesystem::remove_all(DIRECTORY_PATH); 33 | } 34 | 35 | static std::unique_ptr> ad_model_ptr_; 36 | static std::unique_ptr> compiled_model_ptr_; 37 | }; 38 | 39 | std::unique_ptr> MathFunctionsTestModelFixture::ad_model_ptr_ = 40 | nullptr; 41 | std::unique_ptr> 42 | MathFunctionsTestModelFixture::compiled_model_ptr_ = nullptr; 43 | 44 | TEST_F(MathFunctionsTestModelFixture, Evaluation) { 45 | Vector input = Vector::Ones(NUM_INPUT); 46 | 47 | Vector output_expected = evaluate(input); 48 | Vector output_actual = compiled_model_ptr_->evaluate(input); 49 | 50 | EXPECT_TRUE(output_actual.isApprox(output_expected)) 51 | << "Function evaluation is incorrect."; 52 | } 53 | 54 | TEST_F(MathFunctionsTestModelFixture, Jacobian) { 55 | Vector input = Vector::Ones(NUM_INPUT); 56 | 57 | // clang-format off 58 | Matrix J_expected(NUM_OUTPUT, NUM_INPUT); 59 | J_expected << cos(input(0)) * cos(input(1)), -sin(input(0)) * sin(input(1)), 0, 60 | 0, 0, 0.5 / sqrt(input(2)), 61 | 2 * input.transpose(); 62 | // clang-format on 63 | Matrix J_actual = compiled_model_ptr_->jacobian(input); 64 | 65 | EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect."; 66 | } 67 | 68 | TEST_F(MathFunctionsTestModelFixture, Hessian) { 69 | Vector input = Vector::Ones(NUM_INPUT); 70 | 71 | // clang-format off 72 | Matrix H0_expected(NUM_INPUT, NUM_INPUT); 73 | H0_expected << 74 | -sin(input(0)) * cos(input(1)), -cos(input(0)) * sin(input(1)), 0, 75 | -cos(input(0)) * sin(input(1)), -sin(input(0)) * cos(input(1)), 0, 76 | 0, 0, 0; 77 | // clang-format on 78 | 79 | Matrix H1_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT); 80 | H1_expected(2, 2) = -0.25 * pow(input(2), -1.5); 81 | 82 | Matrix H2_expected = 2 * Matrix::Identity(NUM_INPUT, NUM_INPUT); 83 | 84 | Matrix H0_actual = compiled_model_ptr_->hessian(input, 0); 85 | Matrix H1_actual = compiled_model_ptr_->hessian(input, 1); 86 | Matrix H2_actual = compiled_model_ptr_->hessian(input, 2); 87 | 88 | EXPECT_TRUE(H0_actual.isApprox(H0_expected)) 89 | << "Hessian for dim 0 is incorrect."; 90 | EXPECT_TRUE(H1_actual.isApprox(H1_expected)) 91 | << "Hessian for dim 1 is incorrect."; 92 | EXPECT_TRUE(H2_actual.isApprox(H2_expected)) 93 | << "Hessian for dim 2 is incorrect."; 94 | } 95 | 96 | } // namespace MathFunctionsModelTest 97 | } // namespace CppADCodeGenEigenPy 98 | -------------------------------------------------------------------------------- /tests/cpp_tests/MiscTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace CppADCodeGenEigenPy { 6 | namespace MiscModelTest { 7 | 8 | TEST(MiscModelTest, NonexistentLib) { 9 | EXPECT_THROW(CompiledModel model("FakeModel", "nonexistent_path"), 10 | std::runtime_error) 11 | << "No throw when trying to load non-existent library."; 12 | } 13 | 14 | } // namespace MiscModelTest 15 | } // namespace CppADCodeGenEigenPy 16 | -------------------------------------------------------------------------------- /tests/cpp_tests/ParameterizedModelTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "testing/models/ParameterizedTestModel.h" 11 | 12 | namespace CppADCodeGenEigenPy { 13 | namespace ParameterizedModelTest { 14 | 15 | class ParameterizedTestModelFixture : public ::testing::Test { 16 | protected: 17 | using Vector = CompiledModel::Vector; 18 | using Matrix = CompiledModel::Matrix; 19 | 20 | static void SetUpTestSuite() { 21 | // Compile and load our model 22 | boost::filesystem::create_directories(DIRECTORY_PATH); 23 | ad_model_ptr_.reset(new ParameterizedTestModel()); 24 | ad_model_ptr_->compile(MODEL_NAME, DIRECTORY_PATH, 25 | DerivativeOrder::Second); 26 | compiled_model_ptr_.reset( 27 | new CompiledModel(MODEL_NAME, LIB_GENERIC_PATH)); 28 | } 29 | 30 | static void TearDownTestSuite() { 31 | // Delete the compiled shared object. 32 | boost::filesystem::remove_all(DIRECTORY_PATH); 33 | } 34 | 35 | static std::unique_ptr> ad_model_ptr_; 36 | static std::unique_ptr> compiled_model_ptr_; 37 | }; 38 | 39 | std::unique_ptr> 40 | ParameterizedTestModelFixture::ad_model_ptr_ = nullptr; 41 | std::unique_ptr> 42 | ParameterizedTestModelFixture::compiled_model_ptr_ = nullptr; 43 | 44 | TEST_F(ParameterizedTestModelFixture, Evaluation) { 45 | Vector input = 2 * Vector::Ones(NUM_INPUT); 46 | Vector parameters = Vector::Ones(NUM_INPUT); 47 | 48 | Vector output_expected = evaluate(input, parameters); 49 | Vector output_actual = compiled_model_ptr_->evaluate(input, parameters); 50 | EXPECT_TRUE(output_actual.isApprox(output_expected)) 51 | << "Function evaluation is incorrect."; 52 | 53 | // if I forget to pass the parameters, I should get a runtime error 54 | EXPECT_THROW(compiled_model_ptr_->evaluate(input), std::runtime_error) 55 | << "Missing parameters did not throw error."; 56 | } 57 | 58 | TEST_F(ParameterizedTestModelFixture, Jacobian) { 59 | Vector input = 2 * Vector::Ones(NUM_INPUT); 60 | Vector parameters = Vector::Ones(NUM_INPUT); 61 | 62 | // note Jacobian of scalar function is a row vector, hence the transpose 63 | Matrix P = Matrix::Zero(NUM_INPUT, NUM_INPUT); 64 | P.diagonal() << parameters; 65 | Matrix J_expected = input.transpose() * P; 66 | Matrix J_actual = compiled_model_ptr_->jacobian(input, parameters); 67 | EXPECT_TRUE(J_actual.isApprox(J_expected)) << "Jacobian is incorrect."; 68 | 69 | EXPECT_THROW(compiled_model_ptr_->jacobian(input), std::runtime_error) 70 | << "Missing parameters did not throw error."; 71 | } 72 | 73 | TEST_F(ParameterizedTestModelFixture, Hessian) { 74 | Vector input = 2 * Vector::Ones(NUM_INPUT); 75 | Vector parameters = Vector::Ones(NUM_INPUT); 76 | 77 | Matrix H_expected = Matrix::Zero(NUM_INPUT, NUM_INPUT); 78 | H_expected.diagonal() << parameters; 79 | Matrix H_actual = compiled_model_ptr_->hessian(input, parameters, 0); 80 | EXPECT_TRUE(H_actual.isApprox(H_expected)) << "Hessian is incorrect."; 81 | EXPECT_THROW(compiled_model_ptr_->hessian(input, 0), std::runtime_error) 82 | << "Missing parameters did not throw error."; 83 | } 84 | 85 | } // namespace ParameterizedModelTest 86 | } // namespace CppADCodeGenEigenPy 87 | -------------------------------------------------------------------------------- /tests/helpers/compile_models.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "testing/models/BasicTestModel.h" 5 | #include "testing/models/ParameterizedTestModel.h" 6 | #include "testing/models/MathFunctionsTestModel.h" 7 | 8 | using namespace CppADCodeGenEigenPy; 9 | 10 | int main(int argc, char** argv) { 11 | if (argc < 2) { 12 | std::cerr << "Directory path is required." << std::endl; 13 | return 1; 14 | } 15 | std::string directory_path = argv[1]; 16 | BasicModelTest::BasicTestModel().compile( 17 | BasicModelTest::MODEL_NAME, directory_path, DerivativeOrder::Second, 18 | /* verbose = */ true); 19 | BasicModelTest::BasicTestModel().compile( 20 | "LowOrderTestModel", directory_path, DerivativeOrder::Zero, 21 | /* verbose = */ true); 22 | ParameterizedModelTest::ParameterizedTestModel().compile( 23 | ParameterizedModelTest::MODEL_NAME, directory_path, 24 | DerivativeOrder::Second, 25 | /* verbose = */ true); 26 | MathFunctionsModelTest::MathFunctionsTestModel().compile( 27 | MathFunctionsModelTest::MODEL_NAME, directory_path, 28 | DerivativeOrder::Second, 29 | /* verbose = */ true); 30 | } 31 | -------------------------------------------------------------------------------- /tests/include/testing/Defs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace CppADCodeGenEigenPy { 6 | 7 | template 8 | using Vector = Eigen::Matrix; 9 | 10 | } // namespace CppADCodeGenEigenPy 11 | -------------------------------------------------------------------------------- /tests/include/testing/models/BasicTestModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "testing/Defs.h" 9 | 10 | namespace CppADCodeGenEigenPy { 11 | namespace BasicModelTest { 12 | 13 | using Scalar = double; 14 | 15 | const std::string MODEL_NAME = "BasicTestModel"; 16 | const std::string DIRECTORY_PATH = "/tmp/CppADCodeGenEigenPy"; 17 | const std::string LIB_GENERIC_PATH = 18 | get_library_generic_path(MODEL_NAME, DIRECTORY_PATH); 19 | const std::string LIB_REAL_PATH = 20 | get_library_real_path(MODEL_NAME, DIRECTORY_PATH); 21 | 22 | const int NUM_INPUT = 3; 23 | const int NUM_OUTPUT = NUM_INPUT; 24 | 25 | // Just multiply input vector by 2. 26 | template 27 | static Vector evaluate(const Vector& input) { 28 | return input * Scalar(2.); 29 | } 30 | 31 | template 32 | struct BasicTestModel : public ADModel { 33 | using typename ADModel::ADScalar; 34 | using typename ADModel::ADVector; 35 | 36 | // Generate the input to the function 37 | ADVector input() const override { return ADVector::Ones(NUM_INPUT); } 38 | 39 | // Evaluate the function 40 | ADVector function(const ADVector& input) const override { 41 | return evaluate(input); 42 | } 43 | }; 44 | 45 | } // namespace BasicModelTest 46 | } // namespace CppADCodeGenEigenPy 47 | -------------------------------------------------------------------------------- /tests/include/testing/models/MathFunctionsTestModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "testing/Defs.h" 9 | 10 | namespace CppADCodeGenEigenPy { 11 | namespace MathFunctionsModelTest { 12 | 13 | using Scalar = double; 14 | 15 | static const std::string MODEL_NAME = "MathFunctionsTestModel"; 16 | const std::string DIRECTORY_PATH = "/tmp/CppADCodeGenEigenPy"; 17 | const std::string LIB_GENERIC_PATH = 18 | get_library_generic_path(MODEL_NAME, DIRECTORY_PATH); 19 | const std::string LIB_REAL_PATH = 20 | get_library_real_path(MODEL_NAME, DIRECTORY_PATH); 21 | 22 | static const int NUM_INPUT = 3; 23 | static const int NUM_OUTPUT = NUM_INPUT; 24 | 25 | // Use various math routines: trigonometry, square root, vector operations 26 | template 27 | static Vector evaluate(const Vector& input) { 28 | Vector output(NUM_OUTPUT); 29 | output << sin(input(0)) * cos(input(1)), sqrt(input(2)), 30 | input.transpose() * input; 31 | return output; 32 | } 33 | 34 | template 35 | struct MathFunctionsTestModel : public ADModel { 36 | using typename ADModel::ADScalar; 37 | using typename ADModel::ADVector; 38 | 39 | // Generate the input to the function 40 | ADVector input() const override { return ADVector::Ones(NUM_INPUT); } 41 | 42 | // Evaluate the function 43 | ADVector function(const ADVector& input) const override { 44 | return evaluate(input); 45 | } 46 | }; 47 | 48 | } // namespace MathFunctionsModelTest 49 | } // namespace CppADCodeGenEigenPy 50 | -------------------------------------------------------------------------------- /tests/include/testing/models/ParameterizedTestModel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "testing/Defs.h" 9 | 10 | namespace CppADCodeGenEigenPy { 11 | namespace ParameterizedModelTest { 12 | 13 | using Scalar = double; 14 | 15 | const std::string MODEL_NAME = "ParameterizedTestModel"; 16 | const std::string DIRECTORY_PATH = "/tmp/CppADCodeGenEigenPy"; 17 | const std::string LIB_GENERIC_PATH = 18 | get_library_generic_path(MODEL_NAME, DIRECTORY_PATH); 19 | const std::string LIB_REAL_PATH = 20 | get_library_real_path(MODEL_NAME, DIRECTORY_PATH); 21 | 22 | const int NUM_INPUT = 3; 23 | const int NUM_OUTPUT = 1; 24 | const int NUM_PARAM = NUM_INPUT; 25 | 26 | // Parameterized sum of squares. 27 | template 28 | static Vector evaluate(const Vector& input, 29 | const Vector& parameters) { 30 | Vector output(NUM_OUTPUT); 31 | for (int i = 0; i < NUM_INPUT; ++i) { 32 | output(0) += 0.5 * parameters(i) * input(i) * input(i); 33 | } 34 | return output; 35 | } 36 | 37 | // Parameterized weighted sum model. 38 | template 39 | struct ParameterizedTestModel : public ADModel { 40 | using typename ADModel::ADScalar; 41 | using typename ADModel::ADVector; 42 | 43 | // Generate the input to the function 44 | ADVector input() const override { return ADVector::Ones(NUM_INPUT); } 45 | 46 | ADVector parameters() const override { return ADVector::Ones(NUM_PARAM); } 47 | 48 | // Evaluate the function 49 | ADVector function(const ADVector& input, 50 | const ADVector& parameters) const override { 51 | return evaluate(input, parameters); 52 | } 53 | }; 54 | 55 | } // namespace ParameterizedModelTest 56 | } // namespace CppADCodeGenEigenPy 57 | -------------------------------------------------------------------------------- /tests/python_tests/basic_model_test.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | 4 | from CppADCodeGenEigenPy import CompiledModel 5 | 6 | MODEL_NAME = "BasicTestModel" 7 | MODEL_LIB_NAME = "lib" + MODEL_NAME 8 | 9 | NUM_INPUT = 3 10 | NUM_OUTPUT = 3 11 | 12 | 13 | @pytest.fixture 14 | def model(pytestconfig): 15 | lib_path = str( 16 | pytestconfig.rootdir / pytestconfig.getoption("builddir") / MODEL_LIB_NAME 17 | ) 18 | return CompiledModel(MODEL_NAME, lib_path) 19 | 20 | 21 | def test_model_loads(): 22 | # Ensure that failure to load the model actually raises an error 23 | with pytest.raises(RuntimeError): 24 | CompiledModel("nonexistent_model", "nonexistent_path") 25 | 26 | 27 | def test_model_dimensions(model): 28 | assert model.input_size == NUM_INPUT 29 | assert model.output_size == NUM_OUTPUT 30 | 31 | 32 | def test_model_evaluate(model): 33 | x = np.ones(NUM_INPUT) 34 | y_expected = 2 * x 35 | y_actual = model.evaluate(x) 36 | assert np.allclose(y_actual, y_expected) 37 | 38 | # incorrect input size should raise an error 39 | with pytest.raises(RuntimeError): 40 | model.evaluate(np.ones(NUM_INPUT + 1)) 41 | 42 | # same thing with erroneous parameter vector 43 | with pytest.raises(RuntimeError): 44 | model.evaluate(x, np.ones(1)) 45 | 46 | # empty parameter is actually valid (functions are overloaded) 47 | model.evaluate(x, np.ones(0)) 48 | 49 | 50 | def test_model_jacobian(model): 51 | x = np.ones(NUM_INPUT) 52 | J_expected = 2 * np.eye(NUM_INPUT) 53 | J_actual = model.jacobian(x) 54 | assert np.allclose(J_actual, J_expected) 55 | 56 | with pytest.raises(RuntimeError): 57 | model.jacobian(np.ones(NUM_INPUT + 1)) 58 | 59 | with pytest.raises(RuntimeError): 60 | model.jacobian(x, np.ones(1)) 61 | 62 | 63 | def test_model_hessian(model): 64 | x = np.ones(NUM_INPUT) 65 | 66 | H_expected = np.zeros((NUM_INPUT, NUM_INPUT)) 67 | H0_actual = model.hessian(x, 0) 68 | H1_actual = model.hessian(x, 1) 69 | H2_actual = model.hessian(x, 2) 70 | 71 | assert np.allclose(H0_actual, H_expected, atol=1e7) 72 | assert np.allclose(H1_actual, H_expected, atol=1e7) 73 | assert np.allclose(H2_actual, H_expected, atol=1e7) 74 | 75 | with pytest.raises(RuntimeError): 76 | model.hessian(np.ones(NUM_INPUT + 1), 0) 77 | 78 | with pytest.raises(RuntimeError): 79 | model.hessian(x, np.ones(1), 0) 80 | 81 | # invalid output dimension 82 | with pytest.raises(RuntimeError): 83 | model.hessian(x, 3) 84 | -------------------------------------------------------------------------------- /tests/python_tests/low_order_model_test.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | 4 | from CppADCodeGenEigenPy import CompiledModel 5 | 6 | MODEL_NAME = "LowOrderTestModel" 7 | MODEL_LIB_NAME = "lib" + MODEL_NAME 8 | 9 | NUM_INPUT = 3 10 | 11 | 12 | @pytest.fixture 13 | def model(pytestconfig): 14 | lib_path = str( 15 | pytestconfig.rootdir / pytestconfig.getoption("builddir") / MODEL_LIB_NAME 16 | ) 17 | return CompiledModel(MODEL_NAME, lib_path) 18 | 19 | 20 | def test_throws_on_jacobian_hessian(model): 21 | x = np.ones(NUM_INPUT) 22 | 23 | with pytest.raises(RuntimeError): 24 | model.jacobian(x) 25 | 26 | with pytest.raises(RuntimeError): 27 | model.hessian(x, 0) 28 | -------------------------------------------------------------------------------- /tests/python_tests/math_functions_model_test.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | 4 | from CppADCodeGenEigenPy import CompiledModel 5 | 6 | BUILD_DIR_NAME = "build" 7 | MODEL_NAME = "MathFunctionsTestModel" 8 | MODEL_LIB_NAME = "lib" + MODEL_NAME 9 | 10 | NUM_INPUT = 3 11 | NUM_OUTPUT = NUM_INPUT 12 | 13 | 14 | @pytest.fixture 15 | def model(pytestconfig): 16 | lib_path = str( 17 | pytestconfig.rootdir / pytestconfig.getoption("builddir") / MODEL_LIB_NAME 18 | ) 19 | return CompiledModel(MODEL_NAME, lib_path) 20 | 21 | 22 | def test_model_evaluate(model): 23 | x = np.ones(NUM_INPUT) 24 | 25 | y_expected = np.array([np.sin(x[0]) * np.cos(x[1]), np.sqrt(x[2]), x.dot(x)]) 26 | y_actual = model.evaluate(x) 27 | assert np.allclose(y_actual, y_expected) 28 | 29 | 30 | def test_model_jacobian(model): 31 | x = np.ones(NUM_INPUT) 32 | 33 | # fmt: off 34 | J_expected = np.array([ 35 | [np.cos(x[0]) * np.cos(x[1]), -np.sin(x[0]) * np.sin(x[1]), 0], 36 | [0, 0, 0.5 / np.sqrt(x[2])], 37 | [2 * x[0], 2 * x[1], 2 * x[2]], 38 | ]) 39 | # fmt: on 40 | 41 | J_actual = model.jacobian(x) 42 | assert np.allclose(J_actual, J_expected) 43 | 44 | 45 | def test_model_hessian(model): 46 | x = np.ones(NUM_INPUT) 47 | 48 | # fmt: off 49 | H0_expected = np.array([ 50 | [-np.sin(x[0]) * np.cos(x[1]), -np.cos(x[0]) * np.sin(x[1]), 0], 51 | [-np.cos(x[0]) * np.sin(x[1]), -np.sin(x[0]) * np.cos(x[1]), 0], 52 | [0, 0, 0] 53 | ]) 54 | # fmt: on 55 | H1_expected = np.zeros((NUM_INPUT, NUM_INPUT)) 56 | H1_expected[2, 2] = -0.25 * x[2] ** -1.5 57 | 58 | H2_expected = 2 * np.eye(NUM_INPUT) 59 | 60 | H0_actual = model.hessian(x, 0) 61 | H1_actual = model.hessian(x, 1) 62 | H2_actual = model.hessian(x, 2) 63 | 64 | assert np.allclose(H0_actual, H0_expected) 65 | assert np.allclose(H1_actual, H1_expected) 66 | assert np.allclose(H2_actual, H2_expected) 67 | -------------------------------------------------------------------------------- /tests/python_tests/parameterized_model_test.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | 4 | from CppADCodeGenEigenPy import CompiledModel 5 | 6 | BUILD_DIR_NAME = "build" 7 | MODEL_NAME = "ParameterizedTestModel" 8 | MODEL_LIB_NAME = "lib" + MODEL_NAME 9 | 10 | NUM_INPUT = 3 11 | NUM_PARAM = NUM_INPUT 12 | NUM_OUTPUT = 1 13 | 14 | 15 | @pytest.fixture 16 | def model(pytestconfig): 17 | lib_path = str( 18 | pytestconfig.rootdir / pytestconfig.getoption("builddir") / MODEL_LIB_NAME 19 | ) 20 | return CompiledModel(MODEL_NAME, lib_path) 21 | 22 | 23 | def test_model_evaluate(model): 24 | x = 2 * np.ones(NUM_INPUT) 25 | p = np.ones(NUM_INPUT) 26 | y_expected = 0.5 * sum(p * x * x) 27 | y_actual = model.evaluate(x, p) 28 | assert np.allclose(y_actual, y_expected) 29 | 30 | # incorrect input size should raise an error 31 | with pytest.raises(RuntimeError): 32 | model.evaluate(np.ones(NUM_INPUT + 1)) 33 | 34 | # same thing with incorrectly-sized parameter vector 35 | with pytest.raises(RuntimeError): 36 | model.evaluate(x, np.ones(1)) 37 | 38 | # input of size (NUM_INPUT + NUM_PARAM) is actually valid 39 | model.evaluate(np.ones(NUM_INPUT + NUM_PARAM)) 40 | 41 | 42 | def test_model_jacobian(model): 43 | x = 2 * np.ones(NUM_INPUT) 44 | p = np.ones(NUM_PARAM) 45 | P = np.diag(p) 46 | 47 | J_expected = x.dot(P) 48 | J_actual = model.jacobian(x, p) 49 | assert np.allclose(J_actual, J_expected) 50 | 51 | with pytest.raises(RuntimeError): 52 | model.jacobian(np.ones(NUM_INPUT + 1)) 53 | 54 | with pytest.raises(RuntimeError): 55 | model.jacobian(x, np.ones(1)) 56 | 57 | 58 | def test_model_hessian(model): 59 | x = 2 * np.ones(NUM_INPUT) 60 | p = np.ones(NUM_PARAM) 61 | P = np.diag(p) 62 | 63 | H_expected = np.diag(P) 64 | H_actual = model.hessian(x, p, 0) 65 | 66 | assert np.allclose(H_actual, H_expected, atol=1e7) 67 | 68 | with pytest.raises(RuntimeError): 69 | model.hessian(np.ones(NUM_INPUT + 1), 0) 70 | 71 | with pytest.raises(RuntimeError): 72 | model.hessian(x, np.ones(1), 0) 73 | 74 | # invalid output dimension 75 | with pytest.raises(RuntimeError): 76 | model.hessian(x, 3) 77 | --------------------------------------------------------------------------------