Add m_sensors private attribute and sensors() method to iDynTree::Mod…

…el to easily associate a SensorsList with a Model (#1106)
robotology · Sep 7, 2023 · 49f843d · 49f843d
1 parent f0db6c9
commit 49f843d
Show file tree

Hide file tree

Showing 32 changed files with 588 additions and 340 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -20,11 +20,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - The `iDynTree::ModelExporterOptions` class was changed to be defined as a struct (https://github.com/robotology/idyntree/pull/1088).
 - The header structure has been shortened, from `<iDynTree/@component@/@headername@.h>` to `<iDynTree/@headername@.h>` (https://github.com/robotology/idyntree/pull/1104).
 - `iDynTree::idyntree-sensors` library has been merged in `iDynTree::idyntree-model` (https://github.com/robotology/idyntree/pull/1104).
+- The `iDynTree::Model` class gained a `m_sensors` attribute and `sensors()` method to access to it to store sensors information as part of the `iDynTree::Model` itself (https://github.com/robotology/idyntree/pull/1106).
 
 ### Deprecated
 
 - Linking `iDynTree::idyntree-sensors` is deprecated, you can just link `iDynTree::idyntree-model` instead, or just search and replace `iDynTree::idyntree-sensors` with an empty string if you are already linking `iDynTree::idyntree-model` (https://github.com/robotology/idyntree/pull/1104).
 - Including `<iDynTree/Core/@headername@.h>`, `<iDynTree/Model/@headername@.h>`, `<iDynTree/Sensors/@headername@.h>`, `<iDynTree/ModelIO/@headername@.h>`, `<iDynTree/Estimation/@headername@.h>`, `<iDynTree/yarp/@headername@.h>` is deprecated, just include `<iDynTree/@headername@.h>` . To perform this migration, just search and replace `<iDynTree/Core/` with `<iDynTree/`, `<iDynTree/Model/` with `<iDynTree/`, `<iDynTree/Sensors/` with `<iDynTree/`, `<iDynTree/ModelIO/` with `<iDynTree/`, `<iDynTree/Estimation/` with `<iDynTree/`, `<iDynTree/yarp/` with `<iDynTree/` (https://github.com/robotology/idyntree/pull/1104).
+- Several methods that take in input both a `iDynTree::Model` and a `iDynTree::SensorsList` have been deprecated, users are suggested to call the variant that takes in input only the `iDynTree::Model`, if necessary by populating correctly the sensors of the `iDynTree::Model` via the `iDynTree::Model::sensors` method (https://github.com/robotology/idyntree/pull/1106).
 
 
 ## [9.1.0] - 2023-05-25

diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i
@@ -86,6 +86,7 @@
 #include "iDynTree/PrismaticJoint.h"
 #include "iDynTree/Traversal.h"
 #include "iDynTree/SolidShapes.h"
+#include "iDynTree/Sensors.h"
 #include "iDynTree/Model.h"
 #include "iDynTree/JointState.h"
 #include "iDynTree/FreeFloatingMatrices.h"
@@ -94,21 +95,18 @@
 #include "iDynTree/ModelTestUtils.h"
 #include "iDynTree/ModelTransformers.h"
 #include "iDynTree/SubModel.h"
-
-// Kinematics & Dynamics related functions
-#include "iDynTree/ForwardKinematics.h"
-#include "iDynTree/Dynamics.h"
-#include "iDynTree/DenavitHartenberg.h"
-
-// Sensors related data structures
-#include "iDynTree/Sensors.h"
 #include "iDynTree/SixAxisForceTorqueSensor.h"
 #include "iDynTree/AccelerometerSensor.h"
 #include "iDynTree/GyroscopeSensor.h"
 #include "iDynTree/ThreeAxisAngularAccelerometerSensor.h"
 #include "iDynTree/ThreeAxisForceTorqueContactSensor.h"
 #include "iDynTree/PredictSensorsMeasurements.h"
 
+// Kinematics & Dynamics related functions
+#include "iDynTree/ForwardKinematics.h"
+#include "iDynTree/Dynamics.h"
+#include "iDynTree/DenavitHartenberg.h"
+
 // Model loading from external formats
 #include "iDynTree/URDFDofsImport.h"
 #include "iDynTree/ModelLoader.h"
@@ -254,6 +252,7 @@ namespace std {
 %include "iDynTree/PrismaticJoint.h"
 %include "iDynTree/Traversal.h"
 %include "iDynTree/SolidShapes.h"
+%include "iDynTree/Sensors.h"
 %include "iDynTree/Model.h"
 %include "iDynTree/JointState.h"
 %include "iDynTree/FreeFloatingMatrices.h"
@@ -262,7 +261,14 @@ namespace std {
 %include "iDynTree/ModelTestUtils.h"
 %include "iDynTree/ModelTransformers.h"
 %include "iDynTree/SubModel.h"
+%include "iDynTree/SixAxisForceTorqueSensor.h"
+%include "iDynTree/AccelerometerSensor.h"
+%include "iDynTree/GyroscopeSensor.h"
+%include "iDynTree/ThreeAxisAngularAccelerometerSensor.h"
+%include "iDynTree/ThreeAxisForceTorqueContactSensor.h"
+%include "iDynTree/PredictSensorsMeasurements.h"
 
+%include "sensors.i"
 %include "joints.i"
 
 %template(SolidShapesVector) std::vector<iDynTree::SolidShape*>;
@@ -274,17 +280,6 @@ namespace std {
 %include "iDynTree/Dynamics.h"
 %include "iDynTree/DenavitHartenberg.h"
 
-// Sensors related data structures
-%include "iDynTree/Sensors.h"
-%include "iDynTree/SixAxisForceTorqueSensor.h"
-%include "iDynTree/AccelerometerSensor.h"
-%include "iDynTree/GyroscopeSensor.h"
-%include "iDynTree/ThreeAxisAngularAccelerometerSensor.h"
-%include "iDynTree/ThreeAxisForceTorqueContactSensor.h"
-%include "iDynTree/PredictSensorsMeasurements.h"
-
-%include "sensors.i"
-
 // Model loading from external formats
 %include "iDynTree/URDFDofsImport.h"
 %include "iDynTree/ModelLoader.h"

diff --git a/src/estimation/include/iDynTree/BerdyHelper.h b/src/estimation/include/iDynTree/BerdyHelper.h
@@ -14,6 +14,7 @@
 
 #include <iDynTree/Indices.h>
 #include <iDynTree/LinkState.h>
+#include <iDynTree/Model.h>
 #include <iDynTree/Traversal.h>
 #include <iDynTree/FreeFloatingState.h>
 
@@ -341,11 +342,6 @@ class BerdyHelper
      */
     Model m_model;
 
-    /**
-     * Sensors used in this class
-     */
-    SensorsList m_sensors;
-
     /**
      * Traversal used for the dynamics computations
      */
@@ -519,6 +515,7 @@ class BerdyHelper
     /**
      * Access the sensors.
      */
+    IDYNTREE_DEPRECATED_WITH_MSG("Deprecated, please use BerdyHelper::model::sensors method.")
     SensorsList& sensors();
 
     /**
@@ -534,6 +531,7 @@ class BerdyHelper
     /**
      * Access the model (const version).
      */
+    IDYNTREE_DEPRECATED_WITH_MSG("Deprecated, please use BerdyHelper::model::sensors method.")
     const SensorsList& sensors() const;
 
     /**
@@ -543,6 +541,16 @@ class BerdyHelper
      */
     bool isValid() const;
 
+    /**
+     * Init the class
+     *
+     * @param[in] model The used model.
+     * @param[in] options The used options, check BerdyOptions docs.
+     * @return true if all went well, false otherwise.
+     */
+    bool init(const Model& model,
+              const BerdyOptions options=BerdyOptions());
+
     /**
      * Init the class
      *
@@ -551,6 +559,7 @@ class BerdyHelper
      * @param[in] options The used options, check BerdyOptions docs.
      * @return true if all went well, false otherwise.
      */
+    IDYNTREE_DEPRECATED_WITH_MSG("Deprecated, please use the version in which sensors are passed via the iDynTree::Model")
     bool init(const Model& model,
               const SensorsList& sensors,
               const BerdyOptions options=BerdyOptions());

diff --git a/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h b/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h
@@ -51,7 +51,6 @@ class ExtWrenchesAndJointTorquesEstimator
      */
     Model m_model;
     SubModelDecomposition m_submodels;
-    SensorsList m_sensors;
     bool m_isModelValid;
     bool m_isKinematicsUpdated;
 
@@ -95,13 +94,22 @@ class ExtWrenchesAndJointTorquesEstimator
      */
     ~ExtWrenchesAndJointTorquesEstimator();
 
+    /**
+     * \brief Set model and sensors used for the estimation.
+     *
+     * @param[in] _model the kinematic and dynamic model used for the estimation.
+     * @return true if all went well (model and sensors are well formed), false otherwise.
+     */
+    bool setModel(const Model & _model);
+
     /**
      * \brief Set model and sensors used for the estimation.
      *
      * @param[in] _model the kinematic and dynamic model used for the estimation.
      * @param[in] _sensors the sensor model used for the estimation.
      * @return true if all went well (model and sensors are well formed), false otherwise.
      */
+    IDYNTREE_DEPRECATED_WITH_MSG("Deprecated, please use variant of this method (i.e. setModel) where SensorsList is passed via the iDynTree::Model.")
     bool setModelAndSensors(const Model & _model, const SensorsList & _sensors);
 
     /**
@@ -146,6 +154,7 @@ class ExtWrenchesAndJointTorquesEstimator
      *
      * @return the sensor model used for estimation.
      */
+    IDYNTREE_DEPRECATED_WITH_MSG("Deprecated, please use model::sensors.")
     const SensorsList & sensors() const;
 
     /**

diff --git a/src/estimation/src/BerdyHelper.cpp b/src/estimation/src/BerdyHelper.cpp
@@ -158,15 +158,13 @@ BerdyHelper::BerdyHelper(): m_areModelAndSensorsValid(false),
 }
 
 bool BerdyHelper::init(const Model& model,
-                        const SensorsList& sensors,
-                        const BerdyOptions options)
+                       const BerdyOptions options)
 {
     // Reset the class
     m_kinematicsUpdated = false;
     m_areModelAndSensorsValid = false;
 
     m_model = model;
-    m_sensors = sensors;
     m_options = options;
 
     LinkIndex baseLinkIndex;
@@ -234,6 +232,15 @@ bool BerdyHelper::init(const Model& model,
     return res;
 }
 
+bool BerdyHelper::init(const Model& model,
+                       const SensorsList& sensors,
+                       const BerdyOptions options)
+{
+    Model modelCopy = model;
+    modelCopy.sensors() = sensors;
+    return init(modelCopy, options);
+}
+
 BerdyOptions BerdyHelper::getOptions() const
 {
     return m_options;
@@ -250,7 +257,7 @@ bool BerdyHelper::initSensorsMeasurements()
     // Berdy variant is BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES
     if(m_options.berdyVariant != BERDY_FLOATING_BASE_NON_COLLOCATED_EXT_WRENCHES)
     {
-        m_nrOfSensorsMeasurements = m_sensors.getSizeOfAllSensorsMeasurements();
+        m_nrOfSensorsMeasurements = m_model.sensors().getSizeOfAllSensorsMeasurements();
     }
     else
     {
@@ -347,9 +354,9 @@ bool BerdyHelper::initOriginalBerdyFixedBase()
         //        so we need a better way to iterate over them
         if( isLinkSensor(type) )
         {
-            for(size_t sensIdx = 0; sensIdx < m_sensors.getNrOfSensors(type); sensIdx++)
+            for(size_t sensIdx = 0; sensIdx < m_model.sensors().getNrOfSensors(type); sensIdx++)
             {
-                LinkSensor * linkSensor = dynamic_cast<LinkSensor*>(m_sensors.getSensor(type,sensIdx));
+                LinkSensor * linkSensor = dynamic_cast<LinkSensor*>(m_model.sensors().getSensor(type,sensIdx));
 
                 LinkIndex   linkToWhichTheSensorIsAttached = linkSensor->getParentLinkIndex();
 
@@ -575,27 +582,27 @@ IndexRange BerdyHelper::getRangeSensorVariable(const SensorType type, const unsi
 
     if( type > SIX_AXIS_FORCE_TORQUE )
     {
-        sensorOffset += 6*m_sensors.getNrOfSensors(SIX_AXIS_FORCE_TORQUE);
+        sensorOffset += 6*m_model.sensors().getNrOfSensors(SIX_AXIS_FORCE_TORQUE);
     }
 
     if( type > ACCELEROMETER )
     {
-        sensorOffset += 3*m_sensors.getNrOfSensors(ACCELEROMETER);
+        sensorOffset += 3*m_model.sensors().getNrOfSensors(ACCELEROMETER);
     }
 
     if( type > GYROSCOPE )
     {
-        sensorOffset += 3*m_sensors.getNrOfSensors(GYROSCOPE);
+        sensorOffset += 3*m_model.sensors().getNrOfSensors(GYROSCOPE);
     }
 
     if( type > THREE_AXIS_ANGULAR_ACCELEROMETER )
     {
-        sensorOffset += 3*m_sensors.getNrOfSensors(THREE_AXIS_ANGULAR_ACCELEROMETER);
+        sensorOffset += 3*m_model.sensors().getNrOfSensors(THREE_AXIS_ANGULAR_ACCELEROMETER);
     }
 
     if( type > THREE_AXIS_FORCE_TORQUE_CONTACT )
     {
-        sensorOffset += 3*m_sensors.getNrOfSensors(THREE_AXIS_FORCE_TORQUE_CONTACT);
+        sensorOffset += 3*m_model.sensors().getNrOfSensors(THREE_AXIS_FORCE_TORQUE_CONTACT);
     }
 
     sensorOffset += sensorIdx*getSensorTypeSize(type);
@@ -1145,10 +1152,10 @@ bool BerdyHelper::computeBerdySensorsMatricesFromModel(SparseMatrix<iDynTree::Co
     ////////////////////////////////////////////////////////////////////////
     ///// SIX AXIS F/T SENSORS
     ////////////////////////////////////////////////////////////////////////
-    size_t numOfFTs = m_sensors.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
+    size_t numOfFTs = m_model.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
     for(size_t idx = 0; idx<numOfFTs; idx++)
     {
-        SixAxisForceTorqueSensor * ftSens = (SixAxisForceTorqueSensor*)m_sensors.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, idx);
+        SixAxisForceTorqueSensor * ftSens = (SixAxisForceTorqueSensor*)m_model.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, idx);
         LinkIndex childLink;
         childLink = m_dynamicsTraversal.getChildLinkIndexFromJointIndex(m_model,ftSens->getParentJointIndex());
         Matrix6x6 sensor_M_link;
@@ -1165,10 +1172,10 @@ bool BerdyHelper::computeBerdySensorsMatricesFromModel(SparseMatrix<iDynTree::Co
     ////////////////////////////////////////////////////////////////////////
     ///// ACCELEROMETERS
     ////////////////////////////////////////////////////////////////////////
-    unsigned int numAccl = m_sensors.getNrOfSensors(iDynTree::ACCELEROMETER);
+    unsigned int numAccl = m_model.sensors().getNrOfSensors(iDynTree::ACCELEROMETER);
     for(size_t idx = 0; idx<numAccl; idx++)
     {
-        AccelerometerSensor * accelerometer = (AccelerometerSensor *)m_sensors.getSensor(iDynTree::ACCELEROMETER, idx);
+        AccelerometerSensor * accelerometer = (AccelerometerSensor *)m_model.sensors().getSensor(iDynTree::ACCELEROMETER, idx);
         LinkIndex parentLinkId = accelerometer->getParentLinkIndex();
         Transform sensor_X_link = accelerometer->getLinkSensorTransform().inverse();
         IndexRange sensorRange = this->getRangeSensorVariable(ACCELEROMETER,idx);
@@ -1215,10 +1222,10 @@ bool BerdyHelper::computeBerdySensorsMatricesFromModel(SparseMatrix<iDynTree::Co
     ////////////////////////////////////////////////////////////////////////
     ///// GYROSCOPES
     ////////////////////////////////////////////////////////////////////////
-    unsigned int numGyro = m_sensors.getNrOfSensors(iDynTree::GYROSCOPE);
+    unsigned int numGyro = m_model.sensors().getNrOfSensors(iDynTree::GYROSCOPE);
     for(size_t idx = 0; idx<numGyro; idx++)
     {
-        GyroscopeSensor * gyroscope = (GyroscopeSensor*)m_sensors.getSensor(iDynTree::GYROSCOPE, idx);
+        GyroscopeSensor * gyroscope = (GyroscopeSensor*)m_model.sensors().getSensor(iDynTree::GYROSCOPE, idx);
         LinkIndex parentLinkId = gyroscope->getParentLinkIndex();
         Transform sensor_X_link = gyroscope->getLinkSensorTransform().inverse();
         IndexRange sensorRange = this->getRangeSensorVariable(GYROSCOPE,idx);
@@ -1232,10 +1239,10 @@ bool BerdyHelper::computeBerdySensorsMatricesFromModel(SparseMatrix<iDynTree::Co
     ////////////////////////////////////////////////////////////////////////
     ///// THREE AXIS ANGULAR ACCELEROMETERS
     ////////////////////////////////////////////////////////////////////////
-    unsigned int numThreeAxisAngularAccelerometer = m_sensors.getNrOfSensors(iDynTree::THREE_AXIS_ANGULAR_ACCELEROMETER);
+    unsigned int numThreeAxisAngularAccelerometer = m_model.sensors().getNrOfSensors(iDynTree::THREE_AXIS_ANGULAR_ACCELEROMETER);
     for(size_t idx = 0; idx<numThreeAxisAngularAccelerometer; idx++)
     {
-        ThreeAxisAngularAccelerometerSensor * angAccelerometer = (ThreeAxisAngularAccelerometerSensor*)m_sensors.getSensor(iDynTree::THREE_AXIS_ANGULAR_ACCELEROMETER, idx);
+        ThreeAxisAngularAccelerometerSensor * angAccelerometer = (ThreeAxisAngularAccelerometerSensor*)m_model.sensors().getSensor(iDynTree::THREE_AXIS_ANGULAR_ACCELEROMETER, idx);
         LinkIndex parentLinkId = angAccelerometer->getParentLinkIndex();
         Transform sensor_X_link = angAccelerometer->getLinkSensorTransform().inverse();
         IndexRange sensorRange = this->getRangeSensorVariable(THREE_AXIS_ANGULAR_ACCELEROMETER, idx);
@@ -1264,10 +1271,10 @@ bool BerdyHelper::computeBerdySensorsMatricesFromModel(SparseMatrix<iDynTree::Co
     ////////////////////////////////////////////////////////////////////////
     ///// THREE AXIS FORCE TORQUE CONTACT
     ////////////////////////////////////////////////////////////////////////
-    unsigned int numThreeAxisForceTorqueContact = m_sensors.getNrOfSensors(iDynTree::THREE_AXIS_FORCE_TORQUE_CONTACT);
+    unsigned int numThreeAxisForceTorqueContact = m_model.sensors().getNrOfSensors(iDynTree::THREE_AXIS_FORCE_TORQUE_CONTACT);
     for(size_t idx = 0; idx<numThreeAxisForceTorqueContact; idx++)
     {
-        ThreeAxisForceTorqueContactSensor * threeAxisFTContactSensor = (ThreeAxisForceTorqueContactSensor*)m_sensors.getSensor(iDynTree::THREE_AXIS_FORCE_TORQUE_CONTACT, idx);
+        ThreeAxisForceTorqueContactSensor * threeAxisFTContactSensor = (ThreeAxisForceTorqueContactSensor*)m_model.sensors().getSensor(iDynTree::THREE_AXIS_FORCE_TORQUE_CONTACT, idx);
         LinkIndex parentLinkId = threeAxisFTContactSensor->getParentLinkIndex();
         Transform sensor_X_link = threeAxisFTContactSensor->getLinkSensorTransform().inverse();
         IndexRange sensorRange = this->getRangeSensorVariable(THREE_AXIS_FORCE_TORQUE_CONTACT, idx);
@@ -1520,12 +1527,12 @@ const Model& BerdyHelper::model() const
 
 SensorsList& BerdyHelper::sensors()
 {
-    return this->m_sensors;
+    return this->m_model.sensors();
 }
 
 const SensorsList& BerdyHelper::sensors() const
 {
-    return this->m_sensors;
+    return this->m_model.sensors();
 }
 
 const Traversal& BerdyHelper::dynamicTraversal() const
@@ -1728,10 +1735,10 @@ void BerdyHelper::cacheSensorsOrdering()
     //To be a bit more flexible, rely on getRangeSensorVariable to have the order of
     //the URDF sensors
     //???: Isn't this a loop in some way??
-    for (SensorsList::Iterator it = m_sensors.allSensorsIterator();
+    for (SensorsList::Iterator it = m_model.sensors().allSensorsIterator();
          it.isValid(); ++it)
     {
-        IndexRange sensorRange = this->getRangeSensorVariable((*it)->getSensorType(), m_sensors.getSensorIndex((*it)->getSensorType(), (*it)->getName()));
+        IndexRange sensorRange = this->getRangeSensorVariable((*it)->getSensorType(), m_model.sensors().getSensorIndex((*it)->getSensorType(), (*it)->getName()));
 
         BerdySensor sensor;
         sensor.type = static_cast<BerdySensorTypes>((*it)->getSensorType());