Merge pull request #192 from robotology/fix/ergocub246

Fix fatal regression in wholebodydynamics device support of `nonConsideredAxesPositions` parameter

.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

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)

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