diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ae62fc04b0..dc1a4e588e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -68,7 +68,7 @@ jobs: if: matrix.os == 'macOS-latest' run: | brew update - brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt pkg-config + brew install ace boost eigen swig qt5 orocos-kdl catch2 qhull ipopt cppad pkg-config - name: Dependencies [Ubuntu] if: matrix.os == 'ubuntu-latest' @@ -235,6 +235,22 @@ jobs: -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. cmake --build . --config ${{ matrix.build_type }} --target install + - name: CppAD [Ubuntu] + if: matrix.os == 'ubuntu-latest' + shell: bash + run: | + # Catch2 + git clone https://github.com/coin-or/CppAD.git + cd CppAD + mkdir -p build + cd build + cmake -Dcppad_prefix=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DBUILD_TESTING=OFF .. + + cmake --build . --config ${{ matrix.build_type }} --target install + # =================== # CMAKE-BASED PROJECT # =================== diff --git a/CMakeLists.txt b/CMakeLists.txt index a57d7e3ee1..f9a440a3fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,6 +90,10 @@ if (FRAMEWORK_USE_Qhull) list(APPEND FRAMEWORK_PUBLIC_DEPENDENCIES "Qhull 8.0.0") endif() +if (FRAMEWORK_USE_cppad) + list(APPEND FRAMEWORK_PUBLIC_DEPENDENCIES cppad) +endif() + include(InstallBasicPackageFiles) install_basic_package_files(${PROJECT_NAME} NAMESPACE BipedalLocomotion:: diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 4c0a85828e..81e4d28698 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -144,6 +144,9 @@ checkandset_dependency(Qhull) find_package(casadi QUIET) checkandset_dependency(casadi) +find_package(cppad QUIET) +checkandset_dependency(cppad) + framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_USE_YARP" OFF) @@ -167,3 +170,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_ContactModels framework_dependent_option(FRAMEWORK_COMPILE_System "Compile System library?" ON "FRAMEWORK_COMPILE_ContactModels" OFF) + +framework_dependent_option(FRAMEWORK_COMPILE_AutoDiffCppAD + "Compile CppAD-Eigen wrapper?" ON + "FRAMEWORK_USE_cppad" OFF) diff --git a/cmake/Findcppad.cmake b/cmake/Findcppad.cmake new file mode 100644 index 0000000000..e75b1d43a4 --- /dev/null +++ b/cmake/Findcppad.cmake @@ -0,0 +1,96 @@ +#.rst: +# Findcppad +# --------- +# +# Try to locate the cppad library +# +# On non Windows systems, use pkg-config to try to locate the library, +# if this fails then try to locate the library in the directory pointed +# by the cppad_DIR environment variable. +# +# On Windows systems, it is not supported yet +# +# Create the following variables:: +# +# cppad_INCLUDE_DIRS - Directories to include to use cppad +# cppad_LIBRARIES - Default library to link against to use cppad +# cppad_DEFINITIONS - Flags to be added to linker's options +# cppad_LINK_FLAGS - Flags to be added to linker's options +# cppad_FOUND - If false, don't try to use cppad + +#============================================================================= + +if(NOT WIN32) + # On non Windows systems we use PkgConfig to find cppad + find_package(PkgConfig QUIET) + if(PKG_CONFIG_FOUND) + + if(cppad_FIND_VERSION) + if(cppad_FIND_VERSION_EXACT) + pkg_check_modules(_PC_cppad QUIET cppad=${cppad_FIND_VERSION}) + else() + pkg_check_modules(_PC_cppad QUIET cppad>=${cppad_FIND_VERSION}) + endif() + else() + pkg_check_modules(_PC_cppad QUIET cppad) + endif() + + if(_PC_cppad_FOUND) + set(cppad_INCLUDE_DIRS ${_PC_cppad_INCLUDE_DIRS} CACHE PATH "cppad include directory") + set(cppad_DEFINITIONS ${_PC_cppad_CFLAGS} CACHE STRING "Additional compiler flags for cppad") + + find_library(${_PC_cppad_LIBRARIES}_PATH + NAMES ${_PC_cppad_LIBRARIES} + PATHS ${_PC_cppad_LIBRARY_DIRS}) + + set(cppad_LIBRARIES ${${_PC_cppad_LIBRARIES}_PATH} CACHE PATH "cppad libraries" FORCE) + + else() + set(cppad_DEFINITIONS "") + endif() + + endif() + + set(cppad_LINK_FLAGS "") + + # If pkg-config fails, try to find the package using cppad_DIR + if(NOT _PC_cppad_FOUND) + set(cppad_DIR_TEST $ENV{cppad_DIR}) + if(cppad_DIR_TEST) + set(cppad_DIR $ENV{cppad_DIR} CACHE PATH "Path to cppad build directory") + else() + set(cppad_DIR /usr CACHE PATH "Path to cppad build directory") + endif() + + find_path(cppad_INCLUDE_DIRS + NAMES cppad.hpp PATH_SUFFIXES cppad PATHS ${cppad_DIR}/include/cppad) + + find_library(cppad_LIBRARIES cppad ${cppad_DIR}/lib ${cppad_DIR}/lib/cppad) + + set(cppad_DEFINITIONS "") + set(cppad_LINK_FLAGS "") + endif() + +# Windows platforms +else() + message(WARNING "Windows is not supported yet.") +endif() + + +mark_as_advanced(cppad_INCLUDE_DIRS + cppad_LIBRARIES + cppad_DEFINITIONS + cppad_LINK_FLAGS) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(cppad DEFAULT_MSG cppad_LIBRARIES) + +if(NOT cppad_FOUND) + return() +endif() + +if(NOT TARGET cppad) + add_library(cppad UNKNOWN IMPORTED) + set_target_properties(cppad PROPERTIES IMPORTED_LOCATION ${cppad_LIBRARIES}) + set_target_properties(cppad PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${cppad_INCLUDE_DIRS}") +endif() diff --git a/src/AutoDiff/CMakeLists.txt b/src/AutoDiff/CMakeLists.txt new file mode 100644 index 0000000000..d03add6497 --- /dev/null +++ b/src/AutoDiff/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +if(FRAMEWORK_COMPILE_AutoDiffCppAD) + + add_bipedal_locomotion_library( + NAME AutoDiffCppAD + PUBLIC_HEADERS include/BipedalLocomotion/AutoDiff/CppAD.h + PUBLIC_LINK_LIBRARIES Eigen3::Eigen cppad + IS_INTERFACE + SUBDIRECTORIES tests) + +endif() diff --git a/src/AutoDiff/include/BipedalLocomotion/AutoDiff/CppAD.h b/src/AutoDiff/include/BipedalLocomotion/AutoDiff/CppAD.h new file mode 100644 index 0000000000..ad8c879171 --- /dev/null +++ b/src/AutoDiff/include/BipedalLocomotion/AutoDiff/CppAD.h @@ -0,0 +1,64 @@ +/** + * @file CppAD.h + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_AUTODIFF_CPPAD_H +#define BIPEDAL_LOCOMOTION_AUTODIFF_CPPAD_H + +#include +#include +#include + +namespace Eigen +{ +namespace internal +{ + +/** + * cast_impl + * @note Please find more information here: + * https://stackoverflow.com/questions/10088002/how-to-implement-static-cast-in-c/10088161 + */ +template struct cast_impl, Scalar> +{ + // Define eigen function + EIGEN_DEVICE_FUNC + + static inline Scalar run(const CppAD::AD& x) + { + return CppAD::Value(x); + } +}; +} // namespace internal +} // namespace Eigen + +namespace BipedalLocomotion +{ +namespace AutoDiff +{ +namespace CppAD +{ + +// Generate the VectorX and MatrixX typedef for CppAD +#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ + typedef Eigen::Matrix Matrix##SizeSuffix##TypeSuffix; \ + typedef Eigen::Matrix Vector##SizeSuffix##TypeSuffix; \ + typedef Eigen::Matrix RowVector##SizeSuffix##TypeSuffix; + +#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ + EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Eigen::Dynamic, X) + +EIGEN_MAKE_TYPEDEFS_ALL_SIZES(::CppAD::AD, AD) + +#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES +#undef EIGEN_MAKE_TYPEDEFS +#undef EIGEN_MAKE_FIXED_TYPEDEFS + +} // namespace CppAD +} // namespace AutoDiff +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_AUTODIFF_CPPAD_H diff --git a/src/AutoDiff/tests/CMakeLists.txt b/src/AutoDiff/tests/CMakeLists.txt new file mode 100644 index 0000000000..e518305529 --- /dev/null +++ b/src/AutoDiff/tests/CMakeLists.txt @@ -0,0 +1,8 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +add_bipedal_test( + NAME CppADTest + SOURCES CppADTest.cpp + LINKS BipedalLocomotion::AutoDiffCppAD) diff --git a/src/AutoDiff/tests/CppADTest.cpp b/src/AutoDiff/tests/CppADTest.cpp new file mode 100644 index 0000000000..19a46e0ef0 --- /dev/null +++ b/src/AutoDiff/tests/CppADTest.cpp @@ -0,0 +1,44 @@ +/** + * @file CppADTest.cpp + * @authors Giulio Romualdi + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// Catch2 +#include + +#include + +TEST_CASE("CppAD and Eigen") +{ + // Some coefficients useful for the test + constexpr double a = 4; + constexpr double b = 1.1341; + constexpr double c = 2.3213; + + BipedalLocomotion::AutoDiff::CppAD::VectorXAD x(3); + + // Start recording + CppAD::Independent(x); + BipedalLocomotion::AutoDiff::CppAD::VectorXAD y = a * x.array() + c * (b * x.array()).sin(); + + // stop recording + CppAD::ADFun f(x, y); + + Eigen::VectorXd xNum = Eigen::VectorXd::Random(3); + Eigen::VectorXd jac = f.Jacobian(xNum); + + // The Jacobian is stored as a row-major vector + Eigen::Map> jacMatrix(jac.data()); + + // Compute the analytic Jacobian + auto analyticJacobian = [&a, &b](const Eigen::Ref& x) -> Eigen::Matrix3d { + Eigen::Vector3d jacobian; + jacobian = a + c * b * (b * x.array()).cos(); + return jacobian.asDiagonal(); + }; + + constexpr double tolerance = 1e-4; + REQUIRE(jacMatrix.isApprox(analyticJacobian(xNum), tolerance)); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5718fdba1d..22b6455109 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -9,3 +9,4 @@ add_subdirectory(Estimators) add_subdirectory(System) add_subdirectory(Planners) add_subdirectory(ContactModels) +add_subdirectory(AutoDiff)