Skip to content

Commit

Permalink
wholebodydynamics: Remove support for reading FT and IMU using IAnalo…
Browse files Browse the repository at this point in the history 
…gSensor and IGenericSensor interfaces

This support was not explicitly declared deprecated in wholebodydynamics, however the related interfaces
have been deprecated in YARP for a long time. For users interested in using them, please use whole-body-estimators
v0.10.0 . The HW_USE_MAS_IMU group is now compulsory.
  • Loading branch information
@traversaro
traversaro committed Mar 5, 2024
1 parent 6cd3a7a commit 3d79870
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 95 deletions.
87 changes: 5 additions & 82 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -1677,11 +1677,9 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
yInfo()<<"Starting attach MAS and analog ft";
PolyDriverList ftSensorList;
PolyDriverList tempSensorList;
//std::vector<std::string> tempDeviceNames;
std::vector<IAnalogSensor *> ftList;
std::vector<std::string> ftDeviceNames;

std::size_t nrMASFTSensors{0}, nrAnalogFTSensors{0};
std::size_t nrMASFTSensors{0};
for(auto devIdx = 0; devIdx <p.size(); devIdx++)
{
ISixAxisForceTorqueSensors * fts = nullptr;
Expand All @@ -1702,28 +1700,14 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
ftMultipleAnalogSensorNames.emplace_back(ftName);
}
}
// A device is considered an ft if it implements IAnalogSensor and has 6 channels
IAnalogSensor * pAnalogSens = nullptr;
if( p[devIdx]->poly->view(pAnalogSens) )
{
if( pAnalogSens->getChannels() ==static_cast<int>(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<PolyDriverDescriptor&>(*p[devIdx]));
// tempDeviceNames.push_back(p[devIdx]->key);
}
}
yDebug()<<"wholeBodyDynamicsDevice :: number of devices that could contain FT sensors found "<<ftDeviceNames.size()<< "where analog are "<<ftList.size()<<" and MAS are "<<ftSensorList.size();
yDebug()<<"wholeBodyDynamicsDevice :: number of devices that could contain FT sensors found "<<ftDeviceNames.size();

auto totalNrFTDevices{nrAnalogFTSensors + nrMASFTSensors};
auto totalNrFTDevices{nrMASFTSensors};
if( totalNrFTDevices < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) )
{
yError() << "wholeBodyDynamicsDevice : was expecting "
Expand All @@ -1733,8 +1717,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
return false;
}

ftSensors = ftList;
// Attach the controlBoardList to the controlBoardRemapper
// Attach the sensors to the multipleanalogsensorsremapper
bool ok = remappedMASInterfaces.multwrap->attachAll(ftSensorList);
ok = ok & remappedMASInterfaces.multwrap->attachAll(tempSensorList);

Expand Down Expand Up @@ -1853,33 +1836,6 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)

bool WholeBodyDynamicsDevice::attachAllIMUs(const PolyDriverList& p)
{
if (!useMasIMU)
{
std::vector<IGenericSensor*> 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};
Expand Down Expand Up @@ -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){
Expand Down Expand Up @@ -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);
Expand Down
20 changes: 7 additions & 13 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
#include <yarp/dev/IVirtualAnalogSensor.h>
#include <yarp/dev/IAnalogSensor.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/dev/IGenericSensor.h>

// iCub includes
#include <iCub/skinDynLib/skinContactList.h>
Expand Down Expand Up @@ -226,7 +225,6 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver,
} remappedVirtualAnalogSensorsInterfaces;

/** F/T sensors interfaces */
std::vector<yarp::dev::IAnalogSensor * > ftSensors;

/** Remapped multiple analog sensors containing the sensors that implement multiple analog sensor interfaces*/
yarp::dev::PolyDriver multipleAnalogRemappedDevice;
Expand All @@ -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};
Expand Down Expand Up @@ -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);

Expand Down

0 comments on commit 3d79870

Please sign in to comment.