From 58112488281a10aa17d0454720027af6909d7f1b Mon Sep 17 00:00:00 2001
From: Riccardo <riccardo.grieco@hotmail.com>
Date: Tue, 5 Jul 2022 16:23:45 +0200
Subject: [PATCH] Remove includeRcmAsSensor from BerdyOptions

---
 .../include/iDynTree/Estimation/BerdyHelper.h |  9 --------
 src/estimation/src/BerdyHelper.cpp            | 23 ++++---------------
 src/estimation/tests/BerdyHelperUnitTest.cpp  |  2 --
 3 files changed, 5 insertions(+), 29 deletions(-)

diff --git a/src/estimation/include/iDynTree/Estimation/BerdyHelper.h b/src/estimation/include/iDynTree/Estimation/BerdyHelper.h
index a174da56d96..64ae383bfa8 100644
--- a/src/estimation/include/iDynTree/Estimation/BerdyHelper.h
+++ b/src/estimation/include/iDynTree/Estimation/BerdyHelper.h
@@ -165,7 +165,6 @@ struct BerdyOptions
                      includeAllJointAccelerationsAsSensors(true),
                      includeAllJointTorquesAsSensors(false),
                      includeAllNetExternalWrenchesAsSensors(true),
-                     includeRcmAsSensor(false),
                      includeFixedBaseExternalWrench(false),
                      baseLink("")
     {
@@ -216,14 +215,6 @@ struct BerdyOptions
      */
     bool includeAllNetExternalWrenchesAsSensors;
 
-    /*
-     * If true, includes the Rate of Change of Momentum (RCM) in the task sensors vector.
-     * It is compatible only with BERDY_FLOATING_BASE and BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES
-     *
-     * Default value: false .
-     */
-    bool includeRcmAsSensor;
-
     /**
      * Vector of link names that are considered for rate of change of momentum constraint using
      * Rate of Change of Momentum (RCM) sensor
diff --git a/src/estimation/src/BerdyHelper.cpp b/src/estimation/src/BerdyHelper.cpp
index c0f23798273..cfc8e107994 100644
--- a/src/estimation/src/BerdyHelper.cpp
+++ b/src/estimation/src/BerdyHelper.cpp
@@ -130,19 +130,6 @@ bool BerdyOptions::checkConsistency()
         }
     }
 
-    if(this->includeRcmAsSensor &&
-       !(this->berdyVariant==BERDY_FLOATING_BASE || this->berdyVariant==BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES))
-    {
-        reportError("BerdyOptions","checkConsistency","Impossible to load berdy, as includeRcmAsSensor is supported only by BERDY_FLOATING_BASE or BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES variants");
-        return false;
-    }
-
-    if(this->includeRcmAsSensor && !this->includeAllNetExternalWrenchesAsDynamicVariables)
-    {
-        reportError("BerdyOptions","checkConsistency","Impossible to load berdy, as includeRcmAsSensor can be used only if includeAllNetExternalWrenchesAsDynamicVariables is enabled");
-        return false;
-    }
-
     return true;
 }
 
@@ -342,7 +329,7 @@ bool BerdyHelper::initSensorsMeasurements()
     }
 
     // Add Rate of Change of Momentum (RCM) sensor
-    if (m_options.includeRcmAsSensor)
+    if (m_options.berdyVariant==BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES)
     {
         berdySensorTypeOffsets.rcmOffset = m_nrOfSensorsMeasurements;
         m_nrOfSensorsMeasurements += 6;
@@ -1483,7 +1470,7 @@ bool BerdyHelper::computeBerdySensorMatrices(SparseMatrix<iDynTree::ColumnMajor>
     ////////////////////////////////////////////////////////////////////////
     ///// Rate of Change of Momentum (RCM) SENSOR
     ////////////////////////////////////////////////////////////////////////
-    if (m_options.includeRcmAsSensor)
+    if (m_options.berdyVariant==BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES)
     {
         // Get the row index corresponding to the RCM sensor
         IndexRange rcmRange = this->getRangeRCMSensorVariable(RCM_SENSOR);
@@ -1540,7 +1527,7 @@ bool BerdyHelper::initBerdyFloatingBase()
         m_nrOfDynamicEquations   = 6*m_model.getNrOfLinks() + 6*m_model.getNrOfJoints();
     }
 
-    if (m_options.includeRcmAsSensor) {
+    if (m_options.berdyVariant!=BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES) {
         if (m_options.rcmConstraintLinkNamesVector.size() == 0)
         {
             reportInfo("BerdyHelpers","initBerdyFloatingBase","rcmConstraintLinkNamesVector is not initialized using berdy helper options. Considering all the model links");
@@ -1859,7 +1846,7 @@ void BerdyHelper::cacheSensorsOrdering()
         m_sensorsOrdering.push_back(jointSens);
     }
 
-    if(m_options.includeRcmAsSensor) {
+    if(m_options.berdyVariant==BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES) {
 
         IndexRange sensorRange = this->getRangeRCMSensorVariable(RCM_SENSOR);
 
@@ -2311,7 +2298,7 @@ bool BerdyHelper::serializeSensorVariables(SensorsMeasurements& sensMeas,
     ///// Rate of Change of Momentum (RCM)
     ////////////////////////////////////////////////////////////////////////
     /// This method serializeSensorVariables is used in Testing for the Berdy estimator helper class
-    if (m_options.includeRcmAsSensor)
+    if (m_options.berdyVariant==BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES)
     {
         IndexRange sensorRange = this->getRangeRCMSensorVariable(RCM_SENSOR);
 
diff --git a/src/estimation/tests/BerdyHelperUnitTest.cpp b/src/estimation/tests/BerdyHelperUnitTest.cpp
index 30d1f0e79e8..afee37e72a6 100644
--- a/src/estimation/tests/BerdyHelperUnitTest.cpp
+++ b/src/estimation/tests/BerdyHelperUnitTest.cpp
@@ -377,8 +377,6 @@ void testBerdyHelpers(std::string fileName)
 
     // We test the floating base BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES
     options.berdyVariant = iDynTree::BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES;
-    // Include Rate Of Change of Momentum (RCM)
-    options.includeRcmAsSensor = true;
     options.includeAllJointTorquesAsSensors = false;
     options.includeAllJointAccelerationsAsSensors = false;
     options.includeAllNetExternalWrenchesAsSensors = true;