From e82820dafb51cb88e6d93b8df0a12e623e4cde12 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Thu, 2 May 2024 15:36:38 +0200
Subject: [PATCH 1/3] Switch CI to use macos-13

---
 .github/workflows/pixi-ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/pixi-ci.yml b/.github/workflows/pixi-ci.yml
index f4e8113..1bc78c1 100644
--- a/.github/workflows/pixi-ci.yml
+++ b/.github/workflows/pixi-ci.yml
@@ -22,7 +22,7 @@ jobs:
       fail-fast: false
       matrix:
         build_type: [Release]
-        os: [ubuntu-22.04, macos-latest, windows-2019]
+        os: [ubuntu-22.04, macos-13, windows-2019]
 
     steps:
     - uses: actions/checkout@v4

From 64a959f0fc13b4fc9cb78023cd6574f2337f8a60 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Thu, 2 May 2024 15:42:06 +0200
Subject: [PATCH 2/3] Fix fatal regression in wholebodydynamics device

---
 .../WholeBodyDynamicsDevice.cpp               | 41 +++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
index faaa30f..c4b3829 100644
--- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
+++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -15,6 +15,7 @@
 #include <iDynTree/Utils.h>
 #include <iDynTree/EigenHelpers.h>
 #include <iDynTree/ModelLoader.h>
+#include <iDynTree/SixAxisForceTorqueSensor.h>
 
 #include <cassert>
 #include <cmath>
@@ -349,6 +350,20 @@ bool WholeBodyDynamicsDevice::openRemapperVirtualSensors(os::Searchable& config)
     return true;
 }
 
+void getFTJointNames(const iDynTree::Model& model,
+                     std::vector<std::string>& ftJointNames)
+{
+    ftJointNames.resize(0);
+
+    for(size_t sens=0; sens < model.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); sens++)
+    {
+        iDynTree::SixAxisForceTorqueSensor* pSens = static_cast<iDynTree::SixAxisForceTorqueSensor*>(model.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,sens));
+        ftJointNames.push_back(pSens->getParentJoint());
+    }
+
+    return;
+}
+
 bool WholeBodyDynamicsDevice::openEstimator(os::Searchable& config)
 {
     // get the list of considered dofs from config
@@ -382,6 +397,32 @@ bool WholeBodyDynamicsDevice::openEstimator(os::Searchable& config)
     // Return value is ignored as the parameter is not required
     getNonConsideredAxesPositions(config, removedJointPositions);
 
+    // Indipendently of what is passed as additionalConsideredJoints argument, make sure that all the FT sensors
+    // of the full model are included in the reduced model
+    // Solution for regression in https://github.com/icub-tech-iit/ergocub-software/issues/246#issuecomment-2090436940
+    iDynTree::ModelLoader fullModelLoader;
+    ok = fullModelLoader.loadModelFromFile(modelFileFullPath);
+
+    if  (!ok)
+    {
+        yError() << "wholeBodyDynamics : impossible to load iDynTree model from file "
+                 << modelFileName << " ( full path: " << modelFileFullPath << " ) ";
+        return false;
+    }
+
+    // Add FT joints (if they are not already in the consideredDOFs list
+    std::vector< std::string > ftJointNames;
+    getFTJointNames(fullModelLoader.model(),ftJointNames);
+
+    for (size_t i = 0; i < ftJointNames.size(); i++)
+    {
+        // Only add an F/T sensor joint if it is not already in consideredDOFs
+        if (std::find(estimationJointNames.begin(), estimationJointNames.end(), ftJointNames[i]) == estimationJointNames.end())
+        {
+            estimationJointNames.push_back(ftJointNames[i]);
+        }
+    }
+
     iDynTree::ModelLoader mdlLoader;
     mdlLoader.loadReducedModelFromFile(modelFileFullPath, estimationJointNames, removedJointPositions);
 

From faa5ef8b09a8f8c71a7930c159f711ac579be46c Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 2 May 2024 15:27:15 +0200
Subject: [PATCH 3/3] Bump version to 0.11.1

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7b244b8..7f9ec35 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,7 @@
 
 cmake_minimum_required(VERSION 3.5)
 project(whole-body-estimators LANGUAGES CXX
-                              VERSION 0.11.0)
+                              VERSION 0.11.1)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)