Skip to content
This repository has been archived by the owner on Apr 6, 2020. It is now read-only.

Fixed NaN during calibration #243

Merged
merged 2 commits into from
Sep 29, 2017
Merged

Fixed NaN during calibration #243

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fd06d65
Fixed constructor of SixAxisForceTorqueMeasureProcessor
francesco-romano Sep 29, 2017
30ec03f
Added some checks to avoid floating nans
traversaro Sep 27, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ SixAxisForceTorqueMeasureProcessor::SixAxisForceTorqueMeasureProcessor()
{
// Initialize the affice function to be the identity
toEigen(m_secondaryCalibrationMatrix).setIdentity();
toEigen(m_offset).setZero();
m_offset.zero();
}

iDynTree::Matrix6x6& SixAxisForceTorqueMeasureProcessor::secondaryCalibrationMatrix()
Expand Down
16 changes: 16 additions & 0 deletions src/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this check should remain, sanity checking on data read from another software module is a good practice.

{
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
Expand Down