From 054087f969940dcbf50d632f3943d38af93e059e Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Wed, 8 Feb 2023 18:05:24 +0100 Subject: [PATCH 1/5] fix attaching both IMU and FTs as MAS --- .../WholeBodyDynamicsDevice.cpp | 70 +++++++++++-------- 1 file changed, 40 insertions(+), 30 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index edf7fd9..9dd3aa7 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1,3 +1,4 @@ +#include #define SKIN_EVENTS_TIMEOUT 0.2 #include "WholeBodyDynamicsDevice.h" @@ -1687,9 +1688,12 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) ITemperatureSensors *tempS =nullptr; if( p[devIdx]->poly->view(fts) ) { - ftSensorList.push(const_cast(*p[devIdx])); - ftDeviceNames.push_back(p[devIdx]->key); auto nrFTsinThisDevice = fts->getNrOfSixAxisForceTorqueSensors(); + if(nrFTsinThisDevice > 0) + { + ftSensorList.push(const_cast(*p[devIdx])); + ftDeviceNames.push_back(p[devIdx]->key); + } nrMASFTSensors += nrFTsinThisDevice; for (auto ftDx = 0; ftDx < nrFTsinThisDevice; ftDx++) { @@ -1717,7 +1721,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) // tempDeviceNames.push_back(p[devIdx]->key); } } - yDebug()<<"wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas"<poly->view(pAcc) ) { - if (pAcc->getNrOfThreeAxisLinearAccelerometers() != 1) + // Check if the MAS device is actually an IMU = contains one accelerometer + // and the accelerometer's name matches the one in the configuration file + if(pAcc->getNrOfThreeAxisLinearAccelerometers() == 1) { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- Nr acc should be 1"; - return false; - } - - std::string accName; - pAcc->getThreeAxisLinearAccelerometerName(0, accName); - if (accName != masAccName) - { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- acc name mismatch"; - return false; + std::string accName; + pAcc->getThreeAxisLinearAccelerometerName(0, accName); + if (accName == masAccName) + { + masAccInterface = pAcc; + noOfMASDevicesWithOneAcc++; + } } - - masAccInterface = pAcc; } IThreeAxisGyroscopes * pGyro{nullptr}; + // All MAS devices should satisfy this condition if( p[devIdx]->poly->view(pGyro) ) { - if (pGyro->getNrOfThreeAxisGyroscopes() != 1) + // Check if the MAS device is actually an IMU = contains one gyroscope + // and the gyroscope's name matches the one in the configuration file + if(pGyro->getNrOfThreeAxisGyroscopes() == 1) { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR- Nr gyro should be 1"; - return false; - } - - std::string gyroName; - pGyro->getThreeAxisGyroscopeName(0, gyroName); - if (gyroName != masGyroName) - { - yError() << "WholeBodyDynamicsDevice MAS IMU ERROR - gyro name mismatch"; - return false; + std::string gyroName; + pGyro->getThreeAxisGyroscopeName(0, gyroName); + if (gyroName == masGyroName) + { + masGyroInterface = pGyro; + noOfMASDevicesWithOneGyro++; + } } - - masGyroInterface = pGyro; + } + bool foundOneIMUMAS = (noOfMASDevicesWithOneAcc == 1) && (noOfMASDevicesWithOneGyro == 1); + if(foundOneIMUMAS) + { + yInfo()<<"wholeBodyDynamics : Found one IMU multipleAnalogSensor device with accelerometer "<< masAccName << " and gyroscope "<< masGyroName; + break; } } } From fca9e1829ca5c403bd4c87a0a5302466c39c4da7 Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Thu, 9 Feb 2023 09:24:12 +0100 Subject: [PATCH 2/5] add more explicit comments when attaching MAS IMU --- .../WholeBodyDynamicsDevice.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index 9dd3aa7..e3dfa3f 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1744,7 +1744,6 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) return false; } - yDebug() << "Elobaid: remappedMASInterfaces= " << remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors(); if (nrMASFTSensors != remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors()) { yError() << "WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper"; @@ -1922,12 +1921,20 @@ bool WholeBodyDynamicsDevice::attachAllIMUs(const PolyDriverList& p) } } } - bool foundOneIMUMAS = (noOfMASDevicesWithOneAcc == 1) && (noOfMASDevicesWithOneGyro == 1); - if(foundOneIMUMAS) - { - yInfo()<<"wholeBodyDynamics : Found one IMU multipleAnalogSensor device with accelerometer "<< masAccName << " and gyroscope "<< masGyroName; - break; - } + } + + if((noOfMASDevicesWithOneAcc == 1) && (noOfMASDevicesWithOneGyro == 1)) + { + yInfo() << "wholeBodyDynamics : Found one IMU multipleAnalogSensor device with accelerometer " << masAccName << " and gyroscope " << masGyroName; + } + else if((noOfMASDevicesWithOneAcc > 1) || (noOfMASDevicesWithOneGyro > 1)) + { + yError() << "wholeBodyDynamics : Found more than one IMU multipleAnalogSensor devices attached, you need to attach one and only one IMU."; + } + else if((noOfMASDevicesWithOneAcc < 1) || (noOfMASDevicesWithOneGyro < 1)) + { + yError() << "wholeBodyDynamics : Did not find one IMU multipleAnalogSensor devices attached, you need to attach one and only one IMU."; + yError() << "wholeBodyDynamics : In case you are trying to attach an IMU device of the type IGenericSensor, remove the group \"HW_USE_MAS_IMU\" from your config file."; } } if (!settings.disableSensorReadCheckAtStartup) { From bb84cc63e9ed190e7c98ddd35e147e3cdb1e589a Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Thu, 9 Feb 2023 09:31:54 +0100 Subject: [PATCH 3/5] update .gitignore --- .gitignore | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 96261bf..64730cf 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,9 @@ *.out *.app - # Qtcreator project *.user + +# clangd +**/compile_commands.json +**/.cache/ From 529609e1062136a2aab26918212878a76eb0953e Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Thu, 9 Feb 2023 09:36:26 +0100 Subject: [PATCH 4/5] update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 72b55e1..4759c08 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed ### Fixed +- fix attaching both IMU and FTs as MAS in simulation (https://github.com/robotology/whole-body-estimators/pull/167). - Implement `VirtualAnalogClient::getAxes` and `VirtualAnalogRemapper::getAxes` to fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/159, https://github.com/robotology/whole-body-estimators/pull/160). - Fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/166). From 040c6cb78de5d11770acd694756389d3119a7e79 Mon Sep 17 00:00:00 2001 From: Hosameldin Date: Thu, 9 Feb 2023 09:52:32 +0100 Subject: [PATCH 5/5] update CHANGELOG --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4759c08..7bf8b3c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Changed ### Fixed -- fix attaching both IMU and FTs as MAS in simulation (https://github.com/robotology/whole-body-estimators/pull/167). +- fix attaching both IMU and FTs as MAS network wrappers (https://github.com/robotology/whole-body-estimators/pull/167). - Implement `VirtualAnalogClient::getAxes` and `VirtualAnalogRemapper::getAxes` to fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/159, https://github.com/robotology/whole-body-estimators/pull/160). - Fix compilation against YARP 3.8 (https://github.com/robotology/whole-body-estimators/pull/166).