Skip to content

Commit

Permalink
Switch to use iDynTree::Model::sensors() to access the sensors of a m…
Browse files Browse the repository at this point in the history 
…odel
  • Loading branch information
@traversaro
traversaro committed Mar 5, 2024
1 parent 5531e93 commit 999d452
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 41 deletions.
1 change: 0 additions & 1 deletion devices/baseEstimatorV1/include/baseEstimatorV1.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -543,7 +543,6 @@ namespace yarp {

// robot model and sensors
iDynTree::Model m_model; ///< iDynTree object of loaded robot model
iDynTree::SensorsList m_sensors_list; ///< iDynTree object of loaded sensors list from URDF

const double m_sensor_timeout_in_seconds{2.0}; ///< Timeout to check for sensor measurements during dry run initial check
const size_t m_nr_of_channels_in_YARP_IMU_sensor{12}; ///< Number of channels available in YARP IMU sensor output port
Expand Down
21 changes: 10 additions & 11 deletions devices/baseEstimatorV1/src/fbeRobotInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,7 @@ bool yarp::dev::baseEstimatorV1::loadEstimator()
}

m_model = model_loader.model();
m_sensors_list = model_loader.sensors();
m_sensor_measurements.resize(m_sensors_list);
m_sensor_measurements.resize(m_model.sensors());

// check imu relevant information
auto imu_frame_idx = m_model.getFrameIndex(m_imu_name);
Expand Down Expand Up @@ -136,8 +135,8 @@ bool yarp::dev::baseEstimatorV1::loadEstimator()
resizeBuffers();
setPeriod(m_device_period_in_s);

ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_left_foot_ft_sensor, m_left_foot_ft_sensor_index) && ok;
ok = m_sensors_list.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_right_foot_ft_sensor, m_right_foot_ft_sensor_index) && ok;
ok = m_model.sensors().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_left_foot_ft_sensor, m_left_foot_ft_sensor_index) && ok;
ok = m_model.sensors().getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE, m_right_foot_ft_sensor, m_right_foot_ft_sensor_index) && ok;

m_r_sole_R_r_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_right_sole), m_right_foot_ft_sensor_index).getRotation();
m_l_sole_R_l_ft_sensor = m_kin_dyn_comp.getRelativeTransform(m_model.getFrameIndex(m_left_sole), m_left_foot_ft_sensor_index).getRotation();
Expand Down Expand Up @@ -185,16 +184,16 @@ bool yarp::dev::baseEstimatorV1::attachAllForceTorqueSensors(const yarp::dev::Po
}
}

if (ft_sensor_list.size() != m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE))
if (ft_sensor_list.size() != m_model.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE))
{
yError() << "floatingBaseEstimatorV1: " << "Obtained " << m_sensors_list.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) << "from the model, but trying to attach " << (int)ft_sensor_list.size() << " FT sensors in the attach list.";
yError() << "floatingBaseEstimatorV1: " << "Obtained " << m_model.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) << "from the model, but trying to attach " << (int)ft_sensor_list.size() << " FT sensors in the attach list.";
return false;
}

m_whole_body_forcetorque_interface.resize(ft_sensor_list.size());
for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_whole_body_forcetorque_interface.size(); iDyn_sensor_idx++)
{
std::string sensor_name = m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, iDyn_sensor_idx)->getName();
std::string sensor_name = m_model.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, iDyn_sensor_idx)->getName();
// search in sensors list for ft sensor with same name as attach list
int idx_of_device_with_same_name{-1};
for (size_t dev_idx = 0; dev_idx < ft_sensor_list.size(); dev_idx++)
Expand Down Expand Up @@ -262,9 +261,9 @@ bool yarp::dev::baseEstimatorV1::attachAllInertialMeasurementUnits(const yarp::d
for (size_t imu = 0; imu < m_nr_of_IMUs_detected; imu++)
{
bool found_imu{false};
for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_sensors_list.getNrOfSensors(iDynTree::ACCELEROMETER); iDyn_sensor_idx++)
for (size_t iDyn_sensor_idx = 0; iDyn_sensor_idx < m_model.sensors().getNrOfSensors(iDynTree::ACCELEROMETER); iDyn_sensor_idx++)
{
std::string imu_name = m_sensors_list.getSensor(iDynTree::ACCELEROMETER, iDyn_sensor_idx)->getName();
std::string imu_name = m_model.sensors().getSensor(iDynTree::ACCELEROMETER, iDyn_sensor_idx)->getName();
if (imu_sensor_name[imu] == imu_name)
{
found_imu = true;
Expand Down Expand Up @@ -341,7 +340,7 @@ bool yarp::dev::baseEstimatorV1::readFTSensors(bool verbose)

if (!ok && verbose)
{
yWarning() << "floatingBaseEstimatorV1: " << "unable to read from FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " correctly. using old measurements.";
yWarning() << "floatingBaseEstimatorV1: " << "unable to read from FT sensor " << m_model.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " correctly. using old measurements.";
}

bool is_NaN = false;
Expand All @@ -356,7 +355,7 @@ bool yarp::dev::baseEstimatorV1::readFTSensors(bool verbose)

if (is_NaN)
{
yError() << "floatingBaseEstimatorV1: " << "FT sensor " << m_sensors_list.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " contains nan: . using old measurements."<< m_ft_measurements_from_yarp_server.toString();
yError() << "floatingBaseEstimatorV1: " << "FT sensor " << m_model.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE, ft)->getName() << " contains nan: . using old measurements."<< m_ft_measurements_from_yarp_server.toString();
return false;
}

Expand Down
58 changes: 29 additions & 29 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -340,7 +340,7 @@ bool WholeBodyDynamicsDevice::openEstimator(os::Searchable& config)
return false;
}

if( estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) == 0 )
if( estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) == 0 )
{
yWarning() << "wholeBodyDynamics : the loaded model has 0 FT sensors, so the estimation will use just the model.";
yWarning() << "wholeBodyDynamics : If you instead want to add the FT sensors to your model, please check iDynTree documentation on how to add sensors to models.";
Expand Down Expand Up @@ -819,10 +819,10 @@ bool WholeBodyDynamicsDevice::openFilteredFTPorts(os::Searchable& config)

std::string sensorName;
std::string portName;
outputFTPorts.resize(estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE));
for(int ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
outputFTPorts.resize(estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE));
for(int ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
sensorName= estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() ;
sensorName= estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() ;

yInfo() << "wholeBodyDynamics: creating port name and opening port for " << sensorName;

Expand All @@ -848,8 +848,8 @@ void WholeBodyDynamicsDevice::resizeBuffers()
this->measuredContactLocations.resize(estimator.model());
this->ftMeasurement.resize(wholeBodyDynamics_nrOfChannelsOfYARPFTSensor);
this->imuMeasurement.resize(wholeBodyDynamics_nrOfChannelsOfAYARPIMUSensor);
this->rawSensorsMeasurements.resize(estimator.sensors());
this->filteredSensorMeasurements.resize(estimator.sensors());
this->rawSensorsMeasurements.resize(estimator.model().sensors());
this->filteredSensorMeasurements.resize(estimator.model().sensors());
this->estimatedJointTorques.resize(estimator.model());
this->estimatedJointTorquesYARP.resize(this->estimatedJointTorques.size(),0.0);
this->estimateExternalContactWrenches.resize(estimator.model());
Expand All @@ -860,7 +860,7 @@ void WholeBodyDynamicsDevice::resizeBuffers()
jointAccKF.zero();

// Resize F/T stuff
size_t nrOfFTSensors = estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
size_t nrOfFTSensors = estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
calibrationBuffers.calibratingFTsensor.resize(nrOfFTSensors,false);
iDynTree::Wrench zeroWrench;
zeroWrench.zero();
Expand All @@ -870,7 +870,7 @@ void WholeBodyDynamicsDevice::resizeBuffers()
calibrationBuffers.assumedContactLocationsForCalibration.resize(estimator.model());
calibrationBuffers.predictedExternalContactWrenchesForCalibration.resize(estimator.model());
calibrationBuffers.predictedJointTorquesForCalibration.resize(estimator.model());
calibrationBuffers.predictedSensorMeasurementsForCalibration.resize(estimator.sensors());
calibrationBuffers.predictedSensorMeasurementsForCalibration.resize(estimator.model().sensors());

ftProcessors.resize(nrOfFTSensors);

Expand Down Expand Up @@ -1198,9 +1198,9 @@ bool WholeBodyDynamicsDevice::loadSecondaryCalibrationSettingsFromConfig(os::Sea

// Linearly search for the specified sensor
bool sensorFound = false;
for(int ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
for(int ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
if( estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
if( estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
{
yDebug() << "wholeBodyDynamics: using secondary calibration matrix for sensor " << iDynTree_sensorName;

Expand Down Expand Up @@ -1259,10 +1259,10 @@ bool WholeBodyDynamicsDevice::loadTemperatureCoefficientsSettingsFromConfig(os::

// Linearly search for the specified sensor
bool sensorFound = false;
for(auto ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
for(auto ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{

if( estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
if( estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
{
yDebug() << "wholeBodyDynamics: using temperature coefficients for sensor " << iDynTree_sensorName << " temperatureCoeffs values " << temperatureCoeffs.toString();

Expand Down Expand Up @@ -1322,9 +1322,9 @@ bool WholeBodyDynamicsDevice::loadFTSensorOffsetFromConfig(os::Searchable& confi

// Linearly search for the specified sensor
bool sensorFound = false;
for(int ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
for(int ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
if( estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
if( estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() == iDynTree_sensorName )
{
yDebug() << "wholeBodyDynamics: using FT offset"<< ftOffset.toString() <<" for sensor " << iDynTree_sensorName;

Expand Down Expand Up @@ -1724,10 +1724,10 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
yDebug()<<"wholeBodyDynamicsDevice :: number of devices that could contain FT sensors found "<<ftDeviceNames.size()<< "where analog are "<<ftList.size()<<" and MAS are "<<ftSensorList.size();

auto totalNrFTDevices{nrAnalogFTSensors + nrMASFTSensors};
if( totalNrFTDevices < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) )
if( totalNrFTDevices < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) )
{
yError() << "wholeBodyDynamicsDevice : was expecting "
<< estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)
<< estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)
<< " from the model, but got only " << totalNrFTDevices << " FT sensor either using "
<< " analog or MAS interface in the attach list.";
return false;
Expand Down Expand Up @@ -1772,7 +1772,7 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
// Check if the MASremapper and the estimator have a consistent number of ft sensors
int tempSensors = 0;
tempSensors=static_cast<int>( remappedMASInterfaces.temperatureSensors->getNrOfTemperatureSensors());
if( tempSensors > static_cast<int>( estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)) )
if( tempSensors > static_cast<int>( estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)) )
{
yError() << "wholeBodyDynamics : The multipleAnalogRemappedDevice has more sensors than those in the open estimator ft sensors list";
return false;
Expand All @@ -1785,8 +1785,8 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
for (int tSensor=0;tSensor<tempSensors;tSensor++){
remappedMASInterfaces.temperatureSensors->getTemperatureSensorName(tSensor,tempName);
int individualCheck=0;
for (int ft=0;ft<static_cast<int>( estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)); ft++){
ftName=estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
for (int ft=0;ft<static_cast<int>( estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)); ft++){
ftName=estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
if (tempName==ftName){
individualCheck++;
ftMap=ft;
Expand All @@ -1811,10 +1811,10 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)

// iterate through ftMultipleAnalogSensorNames
// check if each name is a FT sensor in the URDF
auto nrFTsInURDF = estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
auto nrFTsInURDF = estimator.model().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();
std::string sensorName = estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,IDTsensIdx)->getName();

bool ftInAnalog = (std::find(ftAnalogSensorNames.begin(), ftAnalogSensorNames.end(), sensorName) != ftAnalogSensorNames.end());
bool ftInMAS = (std::find(ftMultipleAnalogSensorNames.begin(), ftMultipleAnalogSensorNames.end(), sensorName) != ftMultipleAnalogSensorNames.end());
Expand Down Expand Up @@ -2047,9 +2047,9 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)
yarp::dev::MAS_status sensorStatus;
ftMeasurement.resize(6);

for(size_t ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
for(size_t ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
std::string sensorName = estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();

bool ok;
auto ftMasITer = (std::find(ftMultipleAnalogSensorNames.begin(), ftMultipleAnalogSensorNames.end(), sensorName));
Expand Down Expand Up @@ -2101,7 +2101,7 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)

if( !ok && verbose )
{
std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
std::string sensorName = estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
yWarning() << "wholeBodyDynamics warning : FT sensor " << sensorName << " was not readed correctly, using old measurement";
}

Expand All @@ -2116,7 +2116,7 @@ bool WholeBodyDynamicsDevice::readFTSensors(bool verbose)
}
if( isNaN )
{
std::string sensorName = estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
std::string sensorName = estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName();
yError() << "wholeBodyDynamics : FT sensor " << sensorName << " contains nan: " << ftMeasurement.toString() << ", returning error.";
return false;
}
Expand Down Expand Up @@ -2254,7 +2254,7 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
settings.jointAccFilterCutoffInHz);

// Filter and remove offset fromn F/T sensors
for(size_t ft=0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
for(size_t ft=0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++ )
{
iDynTree::Wrench rawFTMeasure;
rawSensorsMeasurements.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,rawFTMeasure);
Expand Down Expand Up @@ -2597,7 +2597,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 < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
for(size_t ft = 0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
{
if( calibrationBuffers.calibratingFTsensor[ft] )
{
Expand All @@ -2622,7 +2622,7 @@ void WholeBodyDynamicsDevice::computeCalibration()
if( calibrationBuffers.nrOfSamplesUsedUntilNowForCalibration >= calibrationBuffers.nrOfSamplesToUseForCalibration )
{
// Compute the offset by averaging the results
for(size_t ft = 0; ft < estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
for(size_t ft = 0; ft < estimator.model().sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); ft++)
{
if( calibrationBuffers.calibratingFTsensor[ft] )
{
Expand All @@ -2631,7 +2631,7 @@ void WholeBodyDynamicsDevice::computeCalibration()
computeMean(calibrationBuffers.measurementSumBuffer[ft],calibrationBuffers.nrOfSamplesUsedUntilNowForCalibration,measurementMean);
computeMean(calibrationBuffers.estimationSumBuffer[ft],calibrationBuffers.nrOfSamplesUsedUntilNowForCalibration,estimationMean);

yInfo() << "wholeBodyDynamics: Offset for sensor " << estimator.sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() << " " << ftProcessors[ft].offset().toString();
yInfo() << "wholeBodyDynamics: Offset for sensor " << estimator.model().sensors().getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,ft)->getName() << " " << ftProcessors[ft].offset().toString();
yInfo() << "wholeBodyDynamics: obtained assuming a measurement of " << measurementMean.asVector().toString() << " and an estimated ft of " << estimationMean.asVector().toString();
}
}
Expand Down

0 comments on commit 999d452

Please sign in to comment.