Introduce MAS Read in whole body estimator

CHANGELOG.md

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Changed
 - Updated the `yarp rpc` command `calibStandingWithJetsiRonCubMk1` to be `calibStandingWithJetsiRonCub`, in order to perform `calibStanding` for the models `iRonCub-Mk1` and `iRonCub-Mk1_1` (See [!136](https://github.com/robotology/whole-body-estimators/pull/136)).
 - Switched default carrier of `genericSensorClient` device from `udp` to `fast_tcp` to avoid delays up to 0.25 seconds when a receiver is interrupted (https://github.com/robotology/whole-body-estimators/pull/145).
+- Introduce MAS Read for FT sensors in `WholeBodyDynamics` device (https://github.com/robotology/whole-body-estimators/pull/153)
 
 ### Fixed
 - Fixed the possibility of deadlock in detachAll method in `wholebodydynamics` and `baseEstimatorV1`. This deadlock could lead to freeze during closing of the yarprobotinterface or the Gazebo simulator if the `gazebo_yarp_robotinterface` plugin was used (https://github.com/robotology/whole-body-estimators/pull/146).

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

@@ -1683,7 +1683,8 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
     //std::vector<std::string>     tempDeviceNames;
     std::vector<IAnalogSensor *> ftList;
     std::vector<std::string>     ftDeviceNames;
-    std::vector<std::string>     ftAnalogSensorNames;
+
+    std::size_t nrMASFTSensors{0}, nrAnalogFTSensors{0};
     for(auto devIdx = 0; devIdx <p.size(); devIdx++)
     {
         ISixAxisForceTorqueSensors * fts = nullptr;
@@ -1692,6 +1693,14 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
         {
             ftSensorList.push(const_cast<PolyDriverDescriptor&>(*p[devIdx]));
             ftDeviceNames.push_back(p[devIdx]->key);
+            auto nrFTsinThisDevice = fts->getNrOfSixAxisForceTorqueSensors();
+            nrMASFTSensors +=  nrFTsinThisDevice;
+            for (auto ftDx = 0; ftDx < nrFTsinThisDevice; ftDx++)
+            {
+                std::string ftName;
+                fts->getSixAxisForceTorqueSensorName(ftDx, ftName);
+                ftMultipleAnalogSensorNames.emplace_back(ftName);
+            }
         }
         // A device is considered an ft if it implements IAnalogSensor and has 6 channels
         IAnalogSensor * pAnalogSens = nullptr;
@@ -1702,6 +1711,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
                 ftList.push_back(pAnalogSens);
                 ftDeviceNames.push_back(p[devIdx]->key);
                 ftAnalogSensorNames.push_back(p[devIdx]->key);
+                nrAnalogFTSensors += 1;
             }
         }
 
@@ -1713,14 +1723,17 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
     }
     yDebug()<<"wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas"<<ftDeviceNames.size()<< "where analog are "<<ftList.size()<<" and mas are "<<ftSensorList.size();
 
-    if( ftList.size() != estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) )
+    auto totalNrFTDevices{nrAnalogFTSensors + nrMASFTSensors};
+    if( totalNrFTDevices < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) )
     {
         yError() << "wholeBodyDynamicsDevice : was expecting "
                  << estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)
-                 << " from the model, but got " << ftList.size() << " FT sensor in the attach list.";
+                 << " from the model, but got only " << totalNrFTDevices << " FT sensor either using "
+                 << " analog or MAS interface in the attach list.";
         return false;
     }
 
+    ftSensors = ftList;
     // Attach the controlBoardList to the controlBoardRemapper
     bool ok = remappedMASInterfaces.multwrap->attachAll(ftSensorList);
     ok = ok & remappedMASInterfaces.multwrap->attachAll(tempSensorList);
@@ -1731,6 +1744,31 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
         return false;
     }
 
+    if (nrMASFTSensors != remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors())
+    {
+        yError() << "WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper";
+        return false;
+    }
+
+    if (nrMASFTSensors != ftMultipleAnalogSensorNames.size())
+    {
+	yError() << "WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensor names";
+        return false;
+    }
+
+    ftMultipleAnalogSensorIdxMapping.resize(ftMultipleAnalogSensorNames.size());
+    for (auto ftDx = 0; ftDx < nrMASFTSensors; ftDx++)
+    {
+        std::string sensorName;
+        remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorName(ftDx, sensorName);
+        auto ftIter = std::find(ftMultipleAnalogSensorNames.begin(), ftMultipleAnalogSensorNames.end(), sensorName);
+        if (ftIter != ftMultipleAnalogSensorNames.end())
+        {
+	    auto ftMapIdx = std::distance(ftMultipleAnalogSensorNames.begin(), ftIter);
+            ftMultipleAnalogSensorIdxMapping[ftMapIdx] = ftDx;
+        }
+    }
+
     // Check if the MASremapper and the estimator have a consistent number of ft sensors
     int tempSensors = 0;
     tempSensors=static_cast<int>( remappedMASInterfaces.temperatureSensors->getNrOfTemperatureSensors());
@@ -1771,32 +1809,21 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
     }
     //End of temporary temperature check
 
-
-    // Old check performed on ft sensors
-    // For now we assume that the name of the F/T sensor device match the sensor name in the URDF
-    // In the future we could use a new fancy sensor interface
-    ftSensors.resize(ftList.size());
-    for(size_t IDTsensIdx=0; IDTsensIdx < ftSensors.size(); IDTsensIdx++)
+    // iterate through ftMultipleAnalogSensorNames
+    // check if each name is a FT sensor in the URDF
+    auto nrFTsInURDF = estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
+    for(size_t IDTsensIdx=0; IDTsensIdx < nrFTsInURDF; IDTsensIdx++)
     {
         std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,IDTsensIdx)->getName();
 
-        // Search for a suitable device
-        int deviceThatHasTheSameNameOfTheSensor = -1;
-        for(size_t deviceIdx = 0; deviceIdx < ftList.size(); deviceIdx++)
-        {
-            if( ftAnalogSensorNames[deviceIdx] == sensorName )
-            {
-                deviceThatHasTheSameNameOfTheSensor = deviceIdx;
-            }
-        }
+        bool ftInAnalog = (std::find(ftAnalogSensorNames.begin(), ftAnalogSensorNames.end(), sensorName) != ftAnalogSensorNames.end());
+        bool ftInMAS = (std::find(ftMultipleAnalogSensorNames.begin(), ftMultipleAnalogSensorNames.end(), sensorName) != ftMultipleAnalogSensorNames.end());
 
-        if( deviceThatHasTheSameNameOfTheSensor == -1 )
+        if (!ftInAnalog && !ftInMAS)
         {
             yError() << "WholeBodyDynamicsDevice was expecting a sensor named " << sensorName << " but it did not find one in the attached devices";
             return false;
         }
-
-        ftSensors[IDTsensIdx] = ftList[deviceThatHasTheSameNameOfTheSensor];
     }
 
     if (!settings.disableSensorReadCheckAtStartup) {
@@ -1819,6 +1846,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
         }
     }
 
+
     return true;
 }
 
@@ -2004,10 +2032,36 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)
     double timeFTStamp=yarp::os::Time::now();
     bool readTemperatureSensorThisTime=false;
     yarp::dev::MAS_status    sensorStatus;
+    ftMeasurement.resize(6);
+
     for(size_t ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
     {
+        std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
+
+        bool ok;
+        auto ftMasITer = (std::find(ftMultipleAnalogSensorNames.begin(), ftMultipleAnalogSensorNames.end(), sensorName));
+        if (ftMasITer != ftMultipleAnalogSensorNames.end())
+        {
+            auto ftID = std::distance(ftMultipleAnalogSensorNames.begin(), ftMasITer);
+            auto sensorId = ftMultipleAnalogSensorIdxMapping[ftID];
+            double timestamp;
+            remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorMeasure(sensorId, ftMeasurement, timestamp);
+
+	    ok = true;
+        }
+        else
+        {
+
+            auto ftAnalogIter = (std::find(ftAnalogSensorNames.begin(), ftAnalogSensorNames.end(), sensorName));
+            if (ftAnalogIter != ftAnalogSensorNames.end())
+            {
+                auto ftID = std::distance(ftMultipleAnalogSensorNames.begin(), ftMasITer);
+                int ftRetVal = ftSensors[ftID]->read(ftMeasurement);
+                ok = (ftRetVal == IAnalogSensor::AS_OK);
+            }
+        }
+
         iDynTree::Wrench bufWrench;
-        int ftRetVal = ftSensors[ft]->read(ftMeasurement);
         if (timeFTStamp-prevFTTempTimeStamp>checkTemperatureEvery_seconds){
             if (ftTempMapping[ft]!=-1){
                 sensorStatus=remappedMASInterfaces.temperatureSensors->getTemperatureSensorStatus(ftTempMapping[ft]);
@@ -2029,7 +2083,6 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)
                 tempMeasurements[ft]=0;
             }
         }
-        bool ok = (ftRetVal == IAnalogSensor::AS_OK);
 
         FTSensorsReadCorrectly = FTSensorsReadCorrectly && ok && TempSensorReadCorrectly;
 
@@ -2531,7 +2584,7 @@ void WholeBodyDynamicsDevice::computeCalibration()
                                                        calibrationBuffers.predictedJointTorquesForCalibration);
 
         // The kinematics information was already set by the readSensorsAndUpdateKinematics method, just compute the offset and add to the buffer
-        for(size_t ft = 0; ft < ftSensors.size(); ft++)
+        for(size_t ft = 0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
         {
             if( calibrationBuffers.calibratingFTsensor[ft] )
             {
@@ -2556,7 +2609,7 @@ void WholeBodyDynamicsDevice::computeCalibration()
         if( calibrationBuffers.nrOfSamplesUsedUntilNowForCalibration >= calibrationBuffers.nrOfSamplesToUseForCalibration )
         {
             // Compute the offset by averaging the results
-            for(size_t ft = 0; ft < ftSensors.size(); ft++)
+            for(size_t ft = 0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
             {
                 if( calibrationBuffers.calibratingFTsensor[ft] )
                 {

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h

@@ -451,6 +451,13 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
      */
     std::vector<wholeBodyDynamics::SixAxisForceTorqueMeasureProcessor> ftProcessors;
 
+    /**
+     * Vector of Analog FT Sensor names
+     */
+    std::vector<std::string>     ftAnalogSensorNames;
+    std::vector<std::string>     ftMultipleAnalogSensorNames;
+    std::vector<int>  ftMultipleAnalogSensorIdxMapping;
+
     /***
      * RPC Calibration related methods
      */