From 30ec03f2256256b39a7ef7069d41e34298582a4e Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 27 Sep 2017 19:39:14 +0200 Subject: [PATCH] Added some checks to avoid floating nans --- .../WholeBodyDynamicsDevice.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index b09f72e1..5a52be65 100644 --- a/src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/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