Added some checks to avoid floating nans

src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

@@ -1088,6 +1088,22 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)
             yWarning() << "wholeBodyDynamics warning : FT sensor " << sensorName << " was not readed correctly, using old measurement";
         }
 
+        bool isNaN = false;
+        for (size_t i = 0; i < ftMeasurement.size(); i++)
+        {
+            if( std::isnan(ftMeasurement[i]) )
+            {
+                isNaN = true;
+                break;
+            }
+        }
+        if( isNaN )
+        {
+            std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
+            yError() << "wholeBodyDynamics : FT sensor " << sensorName << " contains nan: " << ftMeasurement.toString() << ", returning error.";
+            return false;
+        }
+
         if( ok )
         {
             // Format of F/T measurement in YARP/iDynTree is consistent: linear/angular