Extending CI jobs

.github/workflows/conda-ci.yml

@@ -0,0 +1,85 @@
+name: CONDA
+
+on: [push, pull_request]
+
+jobs:
+  crocoddyl-conda:
+    name: (${{ matrix.os }}, ${{ matrix.build_type }})
+    runs-on: ${{ matrix.os }}
+    env:
+      CCACHE_DIR: ${{ matrix.CCACHE_DIR }}
+
+    strategy:
+      fail-fast: false
+      matrix:
+        os: ["ubuntu-latest", "macos-latest"]
+        build_type: [Release, Debug]
+
+        include:
+          - os: ubuntu-latest
+            CCACHE_DIR: /home/runner/.ccache
+          - os: macos-latest
+            CCACHE_DIR: /Users/runner/.ccache
+
+    steps:
+    - uses: actions/checkout@v3
+      with:
+        submodules: recursive
+
+    - uses: actions/cache@v3
+      with:
+        path: ${{ env.CCACHE_DIR }}
+        key: ccache-conda-${{ matrix.os }}-${{ matrix.build_type }}
+
+    - uses: conda-incubator/setup-miniconda@v2
+      with:
+        activate-environment: crocoddyl
+        auto-update-conda: true
+        environment-file: .github/workflows/conda/conda-env.yml
+
+    - name: Install dependencies and update conda
+      shell: bash -l {0}
+      run: |
+        conda activate crocoddyl
+        conda install cmake ccache -c conda-forge
+        conda install llvm-openmp libcxx -c conda-forge
+        conda list
+
+    - name: Install compilers for macOS
+      shell: bash -l {0}
+      if: contains(matrix.os, 'macos-latest')
+      run: |
+        conda install compilers -c conda-forge
+
+    - name: Enable CppADCodeGen compilation
+      shell: bash -l {0}
+      if: contains(matrix.build_type, 'Release')
+      run: |
+        echo "codegen_support=OFF" >> "$GITHUB_ENV"
+
+    - name: Disable CppADCodeGen compilation
+      shell: bash -l {0}
+      if: contains(matrix.build_type, 'Debug')
+      run: |
+        echo "codegen_support=OFF" >> "$GITHUB_ENV"
+
+    - name: Build Crocoddyl
+      shell: bash -l {0}
+      run: |
+        conda activate crocoddyl
+        echo $CONDA_PREFIX
+
+        mkdir build
+        cd build
+
+        cmake .. -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DBUILD_WITH_CODEGEN_SUPPORT=${{ env.codegen_support }} -DPYTHON_EXECUTABLE=$(which python3) -DBUILD_WITH_MULTITHREADS=ON -DINSTALL_DOCUMENTATION=ON -DOpenMP_ROOT=$CONDA_PREFIX
+        make
+        export CTEST_OUTPUT_ON_FAILURE=1
+        make test
+        make install
+
+    - name: Uninstall Crocoddyl
+      shell: bash -l {0}
+      run: |
+        cd build
+        make uninstall

.github/workflows/conda/conda-env.yml

@@ -0,0 +1,18 @@
+name: crocoddyl
+channels:
+  - conda-forge
+  - nodefaults
+dependencies:
+  - boost
+  - numpy
+  - scipy
+  - python
+  - eigen=3.4.0
+  - eigenpy
+  - hpp-fcl
+  - urdfdom
+  - cppad
+  - cppadcodegen
+  - example-robot-data
+  - pinocchio
+  - ipopt

.github/workflows/industrial_ci_action.yml → .github/workflows/ros1-ci.yaml

@@ -1,7 +1,4 @@
-# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
-# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
-
-name: ROS-CI
+name: ROS
 
 # This determines when this workflow is run
 on: [push, pull_request] # on all pushes and PRs
@@ -11,20 +8,12 @@ jobs:
     strategy:
       matrix:
         env:
-          # Ubuntu 20.04, g++, Release
-          - {name: "Focal / g++ / Release", ROS_DISTRO: noetic}
-          # Ubuntu 20.04, clang, Release
-          - {name: "Focal / clang / Release", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++}
-          # Ubuntu 20.04, clang, Release, multi-threading
-          - {name: "Focal / clang / Release / Multi-threading", ROS_DISTRO: noetic, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          - {name: "CI (noetic)", ROS_DISTRO: noetic}
+          - {name: "CI (noetic, clang)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++}
+          - {name: "CI (noetic, clang, multi-threading)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          - {name: "CI (noetic, clang, Debug)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
           # Format check
           #- {name: "Format check", ROS_DISTRO: noetic, CLANG_FORMAT_CHECK: file, CLANG_FORMAT_VERSION: "6.0", BEFORE_RUN_CLANG_FORMAT_CHECK: "wget https://raw.githubusercontent.com/Gepetto/linters/master/.clang-format-6.0 -O /tmp/clang_format_check/crocoddyl/.clang-format", ADDITIONAL_DEBS: wget}
-          ## Working configs for Debug mode. However, tests take too long in Debug mode (~1h on a laptop).
-          ## Hence, not active on GitHub Actions
-          # # Ubuntu 20.04, g++, Debug
-          # - {name: "Focal / g++ / Debug", ROS_DISTRO: noetic, CMAKE_ARGS: '-DCMAKE_BUILD_TYPE=Debug'}
-          # Ubuntu 20.04, clang, Debug
-          - {name: "Focal / clang / Debug", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
     name: ${{ matrix.env.name }}
     env:
       CCACHE_DIR: /github/home/.ccache             # Enable ccache

.github/workflows/ros2-ci.yaml

@@ -0,0 +1,36 @@
+name: ROS
+
+# This determines when this workflow is run
+on: [push, pull_request]
+
+jobs:
+  CI:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: humble}
+          # - {name: "humble, multi-threading", ROS_DISTRO: humble, ADDITIONAL_DEBS: "libomp-dev", CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # - {name: "humble, Debug", ROS_DISTRO: noetic, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
+          - {ROS_DISTRO: rolling}
+          # - {name: "rolling, multi-threading", ROS_DISTRO: rolling, ADDITIONAL_DEBS: "libomp-dev", CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # - {name: "rolling, Debug", ROS_DISTRO: rolling, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
+    env:
+      CCACHE_DIR: /github/home/.ccache             # Enable ccache
+      UPSTREAM_WORKSPACE: dependencies.rosinstall  # to build example-robot-data from source as it's not released via the ROS buildfarm
+      CTEST_OUTPUT_ON_FAILURE: 1
+      BUILDER: colcon
+      # This by-passes issues on importing example_robot_data module when running examples and unit tests.
+      # It seems target_ws is unable to properly overlay upstream_ws.
+      AFTER_SETUP_UPSTREAM_WORKSPACE: 'pip install example-robot-data'
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+        with:
+          submodules: recursive
+      # This step will fetch/store the directory used by ccache before/after the ci run
+      - uses: actions/cache@v3
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{ matrix.env }}

benchmark/CMakeLists.txt

@@ -20,7 +20,7 @@ endforeach(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
 
 if(BUILD_WITH_CODEGEN_SUPPORT)
   foreach(BENCHMARK_NAME ${${PROJECT_NAME}_CODEGEN_BENCHMARK})
-    target_link_libraries(${BENCHMARK_NAME} ${CMAKE_DL_LIBS})
+    target_link_libraries(${BENCHMARK_NAME} ${CMAKE_DL_LIBS} ${cppad_LIBRARY})
   endforeach(BENCHMARK_NAME ${${PROJECT_NAME}_CODEGEN_BENCHMARK})
 endif()
 

include/crocoddyl/core/activations/quadratic.hpp

@@ -1,7 +1,8 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
+//                          Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -39,7 +40,7 @@ class ActivationModelQuadTpl : public ActivationModelAbstractTpl<_Scalar> {
                    << "r has wrong dimension (it should be " +
                           std::to_string(nr_) + ")");
     }
-    data->a_value = (Scalar(0.5) * r.transpose() * r)[0];
+    data->a_value = Scalar(0.5) * r.dot(r);
   };
 
   virtual void calcDiff(const boost::shared_ptr<ActivationDataAbstract>& data,

include/crocoddyl/core/constraint-base.hxx

@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2020-2022, University of Edinburgh, Heriot-Watt University
+// Copyright (C) 2020-2023, University of Edinburgh, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -35,7 +35,7 @@ ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
                     "bigger than the residual dimension.")
   }
   std::size_t max_ng = 2 * (residual_->get_nr() - nh_);
-  if (0 > ng_ || ng_ > max_ng) {
+  if (ng_ > max_ng) {
     throw_pretty("Invalid argument: "
                  << "the number of inequality constraints (ng) is wrong as it "
                     "should be in the range [0, " +

include/crocoddyl/core/numdiff/action.hxx

@@ -115,7 +115,7 @@ void ActionModelNumDiffTpl<Scalar>::calcDiff(
     model_->get_state()->diff(x0, d->data_x[ix]->xnext, d->Fx.col(ix));
     // cost
     data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
-    if (get_with_gauss_approx() > 0) {
+    if (get_with_gauss_approx()) {
       d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
     }
     // constraint
@@ -134,7 +134,7 @@ void ActionModelNumDiffTpl<Scalar>::calcDiff(
     model_->get_state()->diff(x0, d->data_u[iu]->xnext, d->Fu.col(iu));
     // cost
     data->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
-    if (get_with_gauss_approx() > 0) {
+    if (get_with_gauss_approx()) {
       d->Ru.col(iu) = (d->data_u[iu]->r - d->data_0->r) / d->uh_jac;
     }
     // constraint
@@ -230,7 +230,7 @@ void ActionModelNumDiffTpl<Scalar>::calcDiff(
   }
 #endif
 
-  if (get_with_gauss_approx() > 0) {
+  if (get_with_gauss_approx()) {
     data->Lxx = d->Rx.transpose() * d->Rx;
     data->Lxu = d->Rx.transpose() * d->Ru;
     data->Luu = d->Ru.transpose() * d->Ru;
@@ -270,7 +270,7 @@ void ActionModelNumDiffTpl<Scalar>::calcDiff(
     model_->calc(d->data_x[ix], d->xp);
     // cost
     data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
-    if (get_with_gauss_approx() > 0) {
+    if (get_with_gauss_approx()) {
       d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac;
     }
     // constraint
@@ -315,7 +315,7 @@ void ActionModelNumDiffTpl<Scalar>::calcDiff(
   }
 #endif
 
-  if (get_with_gauss_approx() > 0) {
+  if (get_with_gauss_approx()) {
     data->Lxx = d->Rx.transpose() * d->Rx;
   }
 }

package.xml

@@ -14,8 +14,12 @@
   <url type="website">https://github.com/loco-3d/crocoddyl</url>
 
   <build_depend>git</build_depend>
+  <build_depend>doxygen</build_depend>
+  <doc_depend>doxygen</doc_depend>
+  <doc_depend>texlive-latex-base</doc_depend>
   <!-- The following tag is recommended by REP-136 -->
   <exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend>
+  <exec_depend condition="$ROS_VERSION == 2">ament_cmake</exec_depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python</depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3</depend>

unittest/CMakeLists.txt

@@ -85,6 +85,6 @@ endforeach(NAME ${${PROJECT_NAME}_CPP_TESTS})
 
 if(BUILD_WITH_CODEGEN_SUPPORT)
   foreach(TEST_NAME ${${PROJECT_NAME}_CODEGEN_CPP_TESTS})
-    target_link_libraries(${TEST_NAME} ${CMAKE_DL_LIBS})
+    target_link_libraries(${TEST_NAME} ${CMAKE_DL_LIBS} ${cppad_LIBRARY})
   endforeach()
 endif()

unittest/factory/cost.cpp

@@ -1,7 +1,8 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
+//                          Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -14,7 +15,7 @@
 #include "crocoddyl/multibody/residuals/control-gravity.hpp"
 #include "crocoddyl/multibody/residuals/state.hpp"
 // #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
-#include "crocoddyl/core/activations/2norm-barrier.hpp"
+#include "crocoddyl/core/activations/quadratic.hpp"
 #include "crocoddyl/core/costs/cost-sum.hpp"
 #include "crocoddyl/core/utils/exception.hpp"
 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
@@ -222,7 +223,6 @@ boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
   pinocchio::SE3 frame_SE3_obstacle = pinocchio::SE3::Random();
   double alpha = fabs(Eigen::VectorXd::Random(1)[0]);
   double beta = fabs(Eigen::VectorXd::Random(1)[0]);
-  double gamma = fabs(Eigen::VectorXd::Random(1)[0]);
 
   boost::shared_ptr<pinocchio::GeometryModel> geometry =
       boost::make_shared<pinocchio::GeometryModel>(pinocchio::GeometryModel());
@@ -244,8 +244,7 @@ boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
   switch (cost_type) {
     case CostModelCollisionTypes::CostModelResidualPairCollision:
       cost = boost::make_shared<crocoddyl::CostModelResidual>(
-          state,
-          boost::make_shared<crocoddyl::ActivationModel2NormBarrier>(3, gamma),
+          state, boost::make_shared<crocoddyl::ActivationModelQuad>(3),
           boost::make_shared<crocoddyl::ResidualModelPairCollision>(
               state, nu, geometry, 0,
               state->get_pinocchio()->frames[frame_index].parent));

unittest/test_costs_collision.cpp

@@ -249,12 +249,11 @@ bool init_function() {
   // states types.
   for (size_t cost_type = 0; cost_type < CostModelCollisionTypes::all.size();
        ++cost_type) {
-    for (size_t state_type =
-             StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
-         state_type < StateModelTypes::all.size(); ++state_type) {
-      register_cost_model_unit_tests(CostModelCollisionTypes::all[cost_type],
-                                     StateModelTypes::all[state_type]);
-    }
+    register_cost_model_unit_tests(CostModelCollisionTypes::all[cost_type],
+                                   StateModelTypes::StateMultibody_HyQ);
+    register_cost_model_unit_tests(
+        CostModelCollisionTypes::all[cost_type],
+        StateModelTypes::StateMultibody_RandomHumanoid);
   }
   return true;
 }