From f043a356df2d47cbc36b0279c9ae6429e803ed67 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Wed, 8 Jun 2022 17:00:12 +0200 Subject: [PATCH 1/5] wholeBodyDynamicsDevice: Read from MAS FT sensors --- .../WholeBodyDynamicsDevice.cpp | 88 ++++++++++++++----- .../WholeBodyDynamicsDevice.h | 7 ++ 2 files changed, 73 insertions(+), 22 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index 4ca9d4e..cd5ac3e 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1683,7 +1683,8 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) //std::vector tempDeviceNames; std::vector ftList; std::vector ftDeviceNames; - std::vector ftAnalogSensorNames; + + std::size_t nrMASFTSensors{0}, nrAnalogFTSensors{0}; for(auto devIdx = 0; devIdx (*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"<attachAll(ftSensorList); ok = ok & remappedMASInterfaces.multwrap->attachAll(tempSensorList); @@ -1731,6 +1744,25 @@ 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; + } + + 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); + auto ftMapIdx = std::distance(ftMultipleAnalogSensorNames.begin(), ftIter); + if (ftIter != ftMultipleAnalogSensorNames.end()) + { + ftMultipleAnalogSensorIdxMapping[ftMapIdx] = ftDx; + } + } + // Check if the MASremapper and the estimator have a consistent number of ft sensors int tempSensors = 0; tempSensors=static_cast( remappedMASInterfaces.temperatureSensors->getNrOfTemperatureSensors()); @@ -1771,32 +1803,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) { @@ -2006,6 +2027,30 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) yarp::dev::MAS_status sensorStatus; 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 sensorName = ftMultipleAnalogSensorNames[ftID]; + auto sensorId = ftMultipleAnalogSensorIdxMapping[ftID]; + double timestamp; + remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorMeasure(ftID, ftMeasurement, timestamp); + } + 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){ @@ -2029,7 +2074,6 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) tempMeasurements[ft]=0; } } - bool ok = (ftRetVal == IAnalogSensor::AS_OK); FTSensorsReadCorrectly = FTSensorsReadCorrectly && ok && TempSensorReadCorrectly; diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h index 777a646..c3bcdbf 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -451,6 +451,13 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, */ std::vector ftProcessors; + /** + * Vector of Analog FT Sensor names + */ + std::vector ftAnalogSensorNames; + std::vector ftMultipleAnalogSensorNames; + std::vector ftMultipleAnalogSensorIdxMapping; + /*** * RPC Calibration related methods */ From 03da2e3c82af151c9100dc7e595c0d652cd7d079 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Wed, 8 Jun 2022 17:06:32 +0000 Subject: [PATCH 2/5] WIP fix segfaults - to check valid measures --- .../WholeBodyDynamicsDevice.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index cd5ac3e..e881c0c 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1744,21 +1744,30 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) return false; } +yError() << "==============> wholeBodyDynamics:attachAll nrMASFTSensors: " << nrMASFTSensors << " remapper FTS: " << remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors(); 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; + } + +yError() << "===================================> Fino a qui tutto bene....1"; + 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); - auto ftMapIdx = std::distance(ftMultipleAnalogSensorNames.begin(), ftIter); if (ftIter != ftMultipleAnalogSensorNames.end()) { + auto ftMapIdx = std::distance(ftMultipleAnalogSensorNames.begin(), ftIter); ftMultipleAnalogSensorIdxMapping[ftMapIdx] = ftDx; } } @@ -1840,6 +1849,9 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) } } +yError() << "===================================> Fino a qui tutto bene......................9999"; + + return true; } @@ -2025,6 +2037,8 @@ 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(); @@ -2038,6 +2052,10 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) auto sensorId = ftMultipleAnalogSensorIdxMapping[ftID]; double timestamp; remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorMeasure(ftID, ftMeasurement, timestamp); + + yError() << "==================> FT name: " << sensorName << " sensor measure: " << ftMeasurement.toString(); + + ok = true; } else { @@ -2052,7 +2070,7 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) } iDynTree::Wrench bufWrench; - int ftRetVal = ftSensors[ft]->read(ftMeasurement); + //int ftRetVal = ftSensors[ft]->read(ftMeasurement); if (timeFTStamp-prevFTTempTimeStamp>checkTemperatureEvery_seconds){ if (ftTempMapping[ft]!=-1){ sensorStatus=remappedMASInterfaces.temperatureSensors->getTemperatureSensorStatus(ftTempMapping[ft]); From d91dc6b581f3876d6e8ee09515e4a9e1301f3070 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Thu, 9 Jun 2022 09:56:44 +0000 Subject: [PATCH 3/5] Fix bugs in reading from MAS interface --- .../wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index e881c0c..cc9bd29 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1744,7 +1744,6 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) return false; } -yError() << "==============> wholeBodyDynamics:attachAll nrMASFTSensors: " << nrMASFTSensors << " remapper FTS: " << remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors(); if (nrMASFTSensors != remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors()) { yError() << "WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper"; @@ -1757,8 +1756,6 @@ yError() << "==============> wholeBodyDynamics:attachAll nrMASFTSensors: " << nr return false; } -yError() << "===================================> Fino a qui tutto bene....1"; - ftMultipleAnalogSensorIdxMapping.resize(ftMultipleAnalogSensorNames.size()); for (auto ftDx = 0; ftDx < nrMASFTSensors; ftDx++) { @@ -1849,8 +1846,6 @@ yError() << "===================================> Fino a qui tutto bene....1"; } } -yError() << "===================================> Fino a qui tutto bene......................9999"; - return true; } @@ -2048,13 +2043,10 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) if (ftMasITer != ftMultipleAnalogSensorNames.end()) { auto ftID = std::distance(ftMultipleAnalogSensorNames.begin(), ftMasITer); - auto sensorName = ftMultipleAnalogSensorNames[ftID]; auto sensorId = ftMultipleAnalogSensorIdxMapping[ftID]; double timestamp; - remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorMeasure(ftID, ftMeasurement, timestamp); + remappedMASInterfaces.ftMultiSensors->getSixAxisForceTorqueSensorMeasure(sensorId, ftMeasurement, timestamp); - yError() << "==================> FT name: " << sensorName << " sensor measure: " << ftMeasurement.toString(); - ok = true; } else @@ -2593,7 +2585,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] ) { @@ -2618,7 +2610,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] ) { From 18f693e32f0b6d35be154c05d74600c88b0d9e96 Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Thu, 9 Jun 2022 09:58:35 +0000 Subject: [PATCH 4/5] Remove stray commented line --- devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index cc9bd29..65e361a 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -2062,7 +2062,6 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) } iDynTree::Wrench bufWrench; - //int ftRetVal = ftSensors[ft]->read(ftMeasurement); if (timeFTStamp-prevFTTempTimeStamp>checkTemperatureEvery_seconds){ if (ftTempMapping[ft]!=-1){ sensorStatus=remappedMASInterfaces.temperatureSensors->getTemperatureSensorStatus(ftTempMapping[ft]); From b5ac06a01d9f4abbe7f7e2d72faa242182bb024e Mon Sep 17 00:00:00 2001 From: iCubGenova09 Date: Thu, 7 Jul 2022 10:10:07 +0000 Subject: [PATCH 5/5] Update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3af6a61..4eca01d 100644 --- a/CHANGELOG.md +++ b/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).