Implement a Eigen-CppAD skeleton

.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
     # ===================

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::

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)

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()

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()

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 <Eigen/Dense>
+#include <cppad/cppad.hpp>
+#include <cppad/example/cppad_eigen.hpp>
+
+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 <typename Scalar> struct cast_impl<CppAD::AD<Scalar>, Scalar>
+{
+    // Define eigen function
+    EIGEN_DEVICE_FUNC
+
+    static inline Scalar run(const CppAD::AD<Scalar>& 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<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
+    typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix;    \
+    typedef Eigen::Matrix<Type, 1, Size> 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<double>, 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

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)

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 <catch2/catch.hpp>
+
+#include <BipedalLocomotion/AutoDiff/CppAD.h>
+
+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<double> 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<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacMatrix(jac.data());
+
+    // Compute the analytic Jacobian
+    auto analyticJacobian = [&a, &b](const Eigen::Ref<Eigen::Vector3d>& 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));
+}

src/CMakeLists.txt

@@ -9,3 +9,4 @@ add_subdirectory(Estimators)
 add_subdirectory(System)
 add_subdirectory(Planners)
 add_subdirectory(ContactModels)
+add_subdirectory(AutoDiff)