Modify ci to test RobotDynamicsEstimator library (#746)

* Modify ci.yml to compile bayes filters in order to install and test robot dynamics estimator * Use iCubGenova02 instead of iCubGazeboV2_5_plus in the FloatingBaseEstimators tests --------- Co-authored-by: Giulio Romualdi <[email protected]>
ami-iit · Oct 30, 2023 · 85f8928 · 85f8928
1 parent 3fac1a1
commit 85f8928
Show file tree

Hide file tree

Showing 6 changed files with 58 additions and 21 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -22,9 +22,10 @@ env:
   osqp_TAG: v0.6.2
   OsqpEigen_TAG: v0.7.0
   tomlplusplus_TAG: v3.0.1
-  icub_models_TAG: v1.23.3
+  icub_models_TAG: v2.4.0
   UnicyclePlanner_TAG: d3f6c80afe21a9958da769c8dd8a2bbfee5ea922
   telemetry_TAG: v1.2.0
+  bayes_filters_TAG: 53124b1d85fc00c8cd44d572a1e482d7e58c0f50
 
   # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
   VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
@@ -157,7 +158,7 @@ jobs:
       uses: actions/cache@v3
       with:
         path: ${{ github.workspace }}/install/deps
-        key: source-deps-${{ runner.os }}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}-unicycle-${{ env.UnicyclePlanner_TAG }}-icub-models-${{ env.icub_models_TAG }}-robometry-${{ env.telemetry_TAG }}
+        key: source-deps-${{ runner.os }}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}-unicycle-${{ env.UnicyclePlanner_TAG }}-icub-models-${{ env.icub_models_TAG }}-robometry-${{ env.telemetry_TAG }}-bayes-filters-${{ env.bayes_filters_TAG }}
 
 
     - name: Source-based Dependencies [Windows]
@@ -330,6 +331,19 @@ jobs:
                      -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install -j${{env.NUM_CORES_FOR_CMAKE_BUILD}}
 
+        # bayes-filters
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/bayes-filters-lib
+        cd bayes-filters-lib
+        git checkout ${bayes_filters_TAG}
+        mkdir build
+        cd build
+        cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+                     -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \
+                     -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+                     -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install -j${{env.NUM_CORES_FOR_CMAKE_BUILD}}
+
     - name: Source-based Dependencies [Ubuntu/macOS]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && (startsWith(matrix.os, 'ubuntu') || matrix.os == 'macos-latest')
       shell: bash
@@ -468,6 +482,15 @@ jobs:
         cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
         cmake --build . --config ${{ matrix.build_type }} --target install -j${{env.NUM_CORES_FOR_CMAKE_BUILD}}
 
+        # bayes-filters
+        cd ${GITHUB_WORKSPACE}
+        git clone https://github.com/robotology/bayes-filters-lib
+        cd bayes-filters-lib
+        git checkout ${bayes_filters_TAG}
+        mkdir build && cd build
+        cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+        cmake --build . --config ${{ matrix.build_type }} --target install -j${{env.NUM_CORES_FOR_CMAKE_BUILD}}
+
     - name: Source-based Dependencies [Ubuntu]
       if: steps.cache-source-deps.outputs.cache-hit != 'true' && startsWith(matrix.os, 'ubuntu')
       shell: bash
@@ -555,9 +578,9 @@ jobs:
       shell: bash
       run: |
         cd build
-        for missing_dep in YARP Qhull casadi cppad manif Python3 pybind11 pytest matioCpp LieGroupControllers nlohmann_json UnicyclePlanner icub-models; do
+        for missing_dep in YARP Qhull casadi cppad manif Python3 pybind11 pytest matioCpp LieGroupControllers nlohmann_json UnicyclePlanner icub-models BayesFilters; do
             echo "Testing ${missing_dep} as missing dependency."
-            # Deselect missing dependency and build
+            # Deselect missing dependencies and build
             cmake -DFRAMEWORK_USE_${missing_dep}:BOOL=OFF .
             cmake --build . --config ${{ matrix.build_type }} -j${{env.NUM_CORES_FOR_CMAKE_BUILD}}
             # Enable again dependency

diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml
@@ -42,7 +42,7 @@ jobs:
                       liblie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog \
                       nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl \
                       tomlplusplus libunicycle-footstep-planner "icub-models>=1.23.4" \
-                      ros-humble-rclcpp onnxruntime-cpp
+                      ros-humble-rclcpp onnxruntime-cpp libbayes-filters-lib
 
     - name: Linux-only Dependencies [Linux]
       if: contains(matrix.os, 'ubuntu')

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -15,6 +15,7 @@ All notable changes to this project are documented in this file.
 - Change implementation of classes used in `RobotDynamicsEstimator` to optimize performance (https://github.com/ami-iit/bipedal-locomotion-framework/pull/731)
 - CMake: Permit to explictly specify Python installation directory by setting the `FRAMEWORK_PYTHON_INSTALL_DIR` CMake variable (https://github.com/ami-iit/bipedal-locomotion-framework/pull/741)
 - Remove outdated tests for `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/742)
+- Modify CI to install `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/746)
 
 ## [0.15.0] - 2023-09-05
 ### Added

diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp
@@ -7,10 +7,10 @@
 
 #include <Eigen/Dense>
 
+#include <BipedalLocomotion/Conversions/ManifConversions.h>
 #include <BipedalLocomotion/Math/Constants.h>
 #include <BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
-#include <BipedalLocomotion/Conversions/ManifConversions.h>
 
 namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator;
 
@@ -173,42 +173,55 @@ bool RDE::AccelerometerMeasurementDynamics::update()
     {
         if (m_subModelList[subDynamicsIdx].getModel().getNrOfDOFs() > 0)
         {
-            for (int jointIdx = 0; jointIdx < m_subModelList[subDynamicsIdx].getJointMapping().size(); jointIdx++)
+            for (int jointIdx = 0;
+                 jointIdx < m_subModelList[subDynamicsIdx].getJointMapping().size();
+                 jointIdx++)
             {
-                m_subModelJointAcc[subDynamicsIdx][jointIdx] = m_ukfInput.robotJointAccelerations[m_subModelList[subDynamicsIdx].getJointMapping()[jointIdx]];
+                m_subModelJointAcc[subDynamicsIdx][jointIdx]
+                    = m_ukfInput.robotJointAccelerations[m_subModelList[subDynamicsIdx]
+                                                             .getJointMapping()[jointIdx]];
             }
         }
     }
 
-
     for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++)
     {
-        m_subModelBaseAcceleration = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getNuDot().head(6);
+        m_subModelBaseAcceleration
+            = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getNuDot().head(6);
 
-        if (!m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getFrameAcc(m_accelerometerFrameName,
-                                                     m_subModelBaseAcceleration,
-                                                     m_subModelJointAcc[m_subModelsWithAccelerometer[index]],
-                                                     iDynTree::make_span(
-                                                         m_accelerometerFameAcceleration)))
+        if (!m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]
+                 ->getFrameAcc(m_accelerometerFrameName,
+                               m_subModelBaseAcceleration,
+                               m_subModelJointAcc[m_subModelsWithAccelerometer[index]],
+                               iDynTree::make_span(m_accelerometerFameAcceleration.data(),
+                                                   manif::SE3d::Tangent::DoF)))
         {
             log()->error("{} Failed while getting the accelerometer frame acceleration.",
                          errorPrefix);
             return false;
         }
 
-        m_imuRworld = Conversions::toManifRot(m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getWorldTransform(m_accelerometerFrameName).getRotation().inverse());
+        m_imuRworld
+            = Conversions::toManifRot(m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]
+                                          ->getWorldTransform(m_accelerometerFrameName)
+                                          .getRotation()
+                                          .inverse());
 
         m_accRg = m_imuRworld.act(m_gravity);
 
-        m_accelerometerFameVelocity = Conversions::toManifTwist(m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getFrameVel(m_accelerometerFrameName));
+        m_accelerometerFameVelocity = Conversions::toManifTwist(
+            m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getFrameVel(
+                m_accelerometerFrameName));
 
         m_vCrossW = m_accelerometerFameVelocity.lin().cross(m_accelerometerFameVelocity.ang());
 
-        m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_accelerometerFameAcceleration.lin() - m_vCrossW - m_accRg;
+        m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size())
+            = m_accelerometerFameAcceleration.lin() - m_vCrossW - m_accRg;
 
         if (m_useBias)
         {
-            m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_bias;
+            m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size())
+                += m_bias;
         }
     }
 

diff --git a/src/Estimators/tests/FloatingBaseEstimators/FloatingBaseEstimatorTest.cpp b/src/Estimators/tests/FloatingBaseEstimators/FloatingBaseEstimatorTest.cpp
@@ -48,7 +48,7 @@ TEST_CASE("Bare Bones Base Estimator")
     REQUIRE(populateConfig(parameterHandler));
 
     // Load the reduced iDynTree model to be passed to the estimator
-    const std::string model_path = iCubModels::getModelFile("iCubGazeboV2_5_plus");
+    const std::string model_path = iCubModels::getModelFile("iCubGenova02");
     std::cout << model_path << std::endl;
     std::vector<std::string> joints_list = {"neck_pitch", "neck_roll", "neck_yaw",
         "torso_pitch", "torso_roll", "torso_yaw",

diff --git a/src/Estimators/tests/FloatingBaseEstimators/InvariantEKFBaseEstimatorTest.cpp b/src/Estimators/tests/FloatingBaseEstimators/InvariantEKFBaseEstimatorTest.cpp
@@ -119,7 +119,7 @@ TEST_CASE("Invariant EKF Base Estimator")
     REQUIRE(populateConfig(parameterHandler,nr_joints));
 
     // Load the reduced iDynTree model to be passed to the estimator
-    const std::string model_path = iCubModels::getModelFile("iCubGazeboV2_5_plus");
+    const std::string model_path = iCubModels::getModelFile("iCubGenova02");
     std::cout << model_path << std::endl;
 
     // load model using modelComputationsHelper