diff --git a/devices/baseEstimatorV1/include/baseEstimatorV1.h b/devices/baseEstimatorV1/include/baseEstimatorV1.h index d8a0eb3..108c2cc 100644 --- a/devices/baseEstimatorV1/include/baseEstimatorV1.h +++ b/devices/baseEstimatorV1/include/baseEstimatorV1.h @@ -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 diff --git a/devices/baseEstimatorV1/src/fbeRobotInterface.cpp b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp index 3646d84..0be099c 100644 --- a/devices/baseEstimatorV1/src/fbeRobotInterface.cpp +++ b/devices/baseEstimatorV1/src/fbeRobotInterface.cpp @@ -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); @@ -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(); @@ -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++) @@ -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; @@ -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; @@ -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; } diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index a76c7c4..8b519dc 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -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."; @@ -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; @@ -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()); @@ -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(); @@ -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); @@ -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; @@ -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(); @@ -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; @@ -1724,10 +1724,10 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) yDebug()<<"wholeBodyDynamicsDevice :: number of devices that could contain FT sensors found "<( remappedMASInterfaces.temperatureSensors->getNrOfTemperatureSensors()); - if( tempSensors > static_cast( estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE)) ) + if( tempSensors > static_cast( 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; @@ -1785,8 +1785,8 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p) for (int tSensor=0;tSensorgetTemperatureSensorName(tSensor,tempName); int individualCheck=0; - for (int ft=0;ft( 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( 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; @@ -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()); @@ -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)); @@ -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"; } @@ -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; } @@ -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); @@ -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] ) { @@ -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] ) { @@ -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(); } }