diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index e3dfa3f..383e02b 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1117,7 +1117,7 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config) if( !prop.check("HW_USE_MAS_IMU") ) { - useMasIMU = false; + yError() << "wholeBodyDynamics : missing required group parameter HW_USE_MAS_IMU"; } else { @@ -1677,11 +1677,9 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) yInfo()<<"Starting attach MAS and analog ft"; PolyDriverList ftSensorList; PolyDriverList tempSensorList; - //std::vector tempDeviceNames; - std::vector ftList; std::vector ftDeviceNames; - std::size_t nrMASFTSensors{0}, nrAnalogFTSensors{0}; + std::size_t nrMASFTSensors{0}; for(auto devIdx = 0; devIdx poly->view(pAnalogSens) ) - { - if( pAnalogSens->getChannels() ==static_cast(wholeBodyDynamics_nrOfChannelsOfYARPFTSensor) ) - { - ftList.push_back(pAnalogSens); - ftDeviceNames.push_back(p[devIdx]->key); - ftAnalogSensorNames.push_back(p[devIdx]->key); - nrAnalogFTSensors += 1; - } - } - if( p[devIdx]->poly->view(tempS) ) { tempSensorList.push(const_cast(*p[devIdx])); - // tempDeviceNames.push_back(p[devIdx]->key); } } - yDebug()<<"wholeBodyDynamicsDevice :: number of devices that could contain FT sensors found "<attachAll(ftSensorList); ok = ok & remappedMASInterfaces.multwrap->attachAll(tempSensorList); @@ -1853,33 +1836,6 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) bool WholeBodyDynamicsDevice::attachAllIMUs(const PolyDriverList& p) { - if (!useMasIMU) - { - std::vector imuList; - - for(size_t devIdx = 0; devIdx < (size_t)p.size(); devIdx++) - { - IGenericSensor * pGenericSensor = 0; - if( p[devIdx]->poly->view(pGenericSensor) ) - { - imuList.push_back(pGenericSensor); - } - } - - size_t nrOfIMUDetected = imuList.size(); - - if( nrOfIMUDetected != 1 ) - { - yError() << "WholeBodyDynamicsDevice was expecting one and only one IMU, but it found " << nrOfIMUDetected << " in the attached devices"; - return false; - } - - if( imuList.size() == 1 ) - { - this->imuInterface = imuList[0]; - } - } - else { size_t noOfMASDevicesWithOneAcc{0}; size_t noOfMASDevicesWithOneGyro{0}; @@ -2062,17 +2018,6 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose) ok = true; } - else - { - - auto ftAnalogIter = (std::find(ftAnalogSensorNames.begin(), ftAnalogSensorNames.end(), sensorName)); - if (ftAnalogIter != ftAnalogSensorNames.end()) - { - auto ftID = std::distance(ftAnalogSensorNames.begin(), ftAnalogIter); - int ftRetVal = ftSensors[ftID]->read(ftMeasurement); - ok = (ftRetVal == IAnalogSensor::AS_OK); - } - } iDynTree::Wrench bufWrench; if (timeFTStamp-prevFTTempTimeStamp>checkTemperatureEvery_seconds){ @@ -2142,28 +2087,6 @@ bool WholeBodyDynamicsDevice::readIMUSensors(bool verbose) rawIMUMeasurements.angularVel.zero(); bool ok{false}; - if (!useMasIMU) - { - ok = imuInterface->read(imuMeasurement); - - if( !ok && verbose ) - { - yWarning() << "wholeBodyDynamics warning : imu sensor was not readed correctly, using old measurement"; - } - - if( ok ) - { - // Check format of IMU in YARP http://wiki.icub.org/wiki/Inertial_Sensor - rawIMUMeasurements.angularVel(0) = deg2rad(imuMeasurement[6]); - rawIMUMeasurements.angularVel(1) = deg2rad(imuMeasurement[7]); - rawIMUMeasurements.angularVel(2) = deg2rad(imuMeasurement[8]); - - rawIMUMeasurements.linProperAcc(0) = imuMeasurement[3]; - rawIMUMeasurements.linProperAcc(1) = imuMeasurement[4]; - rawIMUMeasurements.linProperAcc(2) = imuMeasurement[5]; - } - } - else { double time_stamp; yarp::sig::Vector acc(3), gyro(3); diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h index 32850b3..d0c7649 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -13,7 +13,6 @@ #include #include #include -#include // iCub includes #include @@ -226,7 +225,6 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, } remappedVirtualAnalogSensorsInterfaces; /** F/T sensors interfaces */ - std::vector ftSensors; /** Remapped multiple analog sensors containing the sensors that implement multiple analog sensor interfaces*/ yarp::dev::PolyDriver multipleAnalogRemappedDevice; @@ -240,8 +238,6 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, double prevFTTempTimeStamp; /** IMU interface */ - yarp::dev::IGenericSensor * imuInterface; - yarp::dev::IThreeAxisLinearAccelerometers* masAccInterface; yarp::dev::IThreeAxisGyroscopes* masGyroInterface; bool useMasIMU{true}; @@ -316,21 +312,19 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, /** * Attach all Six Axis Force/Torque devices. * A device is identified as a Six Axis F/T if it - * implements the IAnalogSensor interface. It also ataches MAS Six Axis F/T sensors. - * A device is identified as a MAS Six Axis F/T if it - * implements the Multiple Analog Sensor interfaces. + * implements the ISixAxisForceTorqueSensors interface + * and getNrOfSixAxisForceTorqueSensors() >= 1 . + * If the device also implements the ITemperatureSensors + * interface, that is handled. */ bool attachAllFTs(const PolyDriverList& p); /** * Attach all IMU devices. * A device is identified as an IMU if it - * implements the IGenericSensor interface - * OR (it implements - * the IAnalogSensor interface AND it has 12 channels). - * (The first is the case of FT sensors in the yarprobotinterface - * and the second is in the case of AnalogSensorClient). - * + * implements the IThreeAxisLinearAccelerometers and + * IThreeAxisGyroscopes interfaces, and getNrOfThreeAxisLinearAccelerometers + * and getNrOfThreeAxisGyroscopes return 1. */ bool attachAllIMUs(const PolyDriverList& p);