Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

wholebodydynamics: Remove support for reading FT and IMU using IAnalogSensor and IGenericSensor interfaces #181

Merged
merged 4 commits into from
Mar 6, 2024
Merged

wholebodydynamics: Remove support for reading FT and IMU using IAnalogSensor and IGenericSensor interfaces #181

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3d79870
wholebodydynamics: Remove support for reading FT and IMU using IAnalo…
traversaro Mar 5, 2024
6938303
Update docs
traversaro Mar 5, 2024
3d5ff94
Add missing return false
traversaro Mar 6, 2024
df5ab03
Merge branch 'master' into fix179
traversaro Mar 6, 2024
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 devices/wholeBodyDynamics/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
| | enableGravityCompensation | bool | - | - | No | | |
| | gravityCompensationBaseLink| string | - | - | No | .. | |
| | gravityCompensationAxesNames | vector of strings | - | - | No | Axes for which the gravity compensation is published. | |
| HW_USE_MAS_IMU | - | group | - | - | No | Group to enable attach to Multiple Analog Sensor interface based IMU. | If the group is not present, the default behavior of attaching to IGenericSensor interface IMU will be carried out. |
| HW_USE_MAS_IMU | - | group | - | - | Yes | Group to enable attach to Multiple Analog Sensor interface based IMU. | The group is compulsory since whole-body-estimators 0.11.0 . |
| | accelerometer | string | - | - | Yes | Should match the sensor id used to open the device containing MAS IThreeAxisLinearAcccelerometers interface | |
| | gyroscope | string | - | - | Yes | Should match the sensor id used to open the device containing MAS IThreeAxisGyroscopes interface | |

Expand Down
88 changes: 6 additions & 82 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1117,7 +1117,8 @@ 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";
traversaro marked this conversation as resolved.
Show resolved Hide resolved
return false;
}
else
{
Expand Down Expand Up @@ -1677,11 +1678,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 +1701,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 +1718,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 +1837,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 +2019,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 +2088,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
{
Copy link
Contributor

Choose a reason for hiding this comment

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

This parenthesis "{" is not needed anymore since the if-else has been deleted

Copy link
Member Author

Choose a reason for hiding this comment

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

Yes, I know, but I did not want to add unnecessary noise in the PR, with all those lines that just changed due to indentation.

double time_stamp;
yarp::sig::Vector acc(3), gyro(3);
Expand Down
20 changes: 7 additions & 13 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
View file
Open in desktop
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.
Copy link
Contributor

Choose a reason for hiding this comment

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

that is handled ?

Copy link
Member Author

Choose a reason for hiding this comment

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

Not sure about this comment, what do you mean? That the phrase is not clear?

Copy link
Contributor

Choose a reason for hiding this comment

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

Sorry @traversaro I missed this message !I was not super sure about the meaning of the sentence but it is fine :)

Copy link
Member Author

Choose a reason for hiding this comment

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

Ack, I will change it in a future PR then. I meant. If the device also implements the yarp::dev::ITemperatureSensors interface, this interface will be viewed and configured in this method.

*/
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
Loading