From e12a5447b1e92231dcbd8f2de8debb2d73006454 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 5 Mar 2024 22:22:16 +0100 Subject: [PATCH 1/3] Consolidate find_package calls and required iDynTree 10.0.0 --- CMakeLists.txt | 6 ++++-- devices/baseEstimatorV1/CMakeLists.txt | 10 ---------- devices/genericSensorClient/CMakeLists.txt | 2 -- devices/virtualAnalogClient/CMakeLists.txt | 3 --- devices/virtualAnalogRemapper/CMakeLists.txt | 2 -- devices/wholeBodyDynamics/CMakeLists.txt | 4 ---- libraries/ctrlLibRT/CMakeLists.txt | 2 -- 7 files changed, 4 insertions(+), 25 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c60ddf..4989885 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,8 +30,10 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BIND set(YARP_REQUIRED_VERSION 3.0.1) -find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED COMPONENTS robotinterface idl_tools) -find_package(Eigen3 3.2.92 REQUIRED) +find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED COMPONENTS robotinterface idl_tools eigen os sig) +find_package(Eigen3 REQUIRED) +find_package(iDynTree 10.0.0 REQUIRED) +find_package(ICUB REQUIRED) yarp_configure_plugins_installation(${PROJECT_NAME}) diff --git a/devices/baseEstimatorV1/CMakeLists.txt b/devices/baseEstimatorV1/CMakeLists.txt index 9a998e2..918f8de 100644 --- a/devices/baseEstimatorV1/CMakeLists.txt +++ b/devices/baseEstimatorV1/CMakeLists.txt @@ -2,16 +2,6 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -set(iDynTree_REQUIRED_VERSION 0.11.0) - -find_package(iDynTree ${iDynTree_VERSION} REQUIRED) - -find_package(ICUB REQUIRED) - -find_package(YARP REQUIRED COMPONENTS - eigen - os - sig) set(FBE_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/include/baseEstimatorV1.h ${CMAKE_CURRENT_SOURCE_DIR}/include/WalkingLogger.tpp diff --git a/devices/genericSensorClient/CMakeLists.txt b/devices/genericSensorClient/CMakeLists.txt index 6a7ec89..7505403 100644 --- a/devices/genericSensorClient/CMakeLists.txt +++ b/devices/genericSensorClient/CMakeLists.txt @@ -2,8 +2,6 @@ # Authors: Silvio Traversaro # CopyPolicy: Released under the terms of the GNU LGPL v2+ -find_package(YARP REQUIRED) - yarp_prepare_plugin(genericSensorClient CATEGORY device TYPE yarp::dev::GenericSensorClient INCLUDE "GenericSensorClient.h" diff --git a/devices/virtualAnalogClient/CMakeLists.txt b/devices/virtualAnalogClient/CMakeLists.txt index f4f034a..96140ab 100644 --- a/devices/virtualAnalogClient/CMakeLists.txt +++ b/devices/virtualAnalogClient/CMakeLists.txt @@ -2,9 +2,6 @@ # Authors: Silvio Traversaro # CopyPolicy: Released under the terms of the GNU LGPL v2+ -find_package(YARP REQUIRED) - - yarp_prepare_plugin(virtualAnalogClient CATEGORY device TYPE yarp::dev::VirtualAnalogClient INCLUDE "VirtualAnalogClient.h" diff --git a/devices/virtualAnalogRemapper/CMakeLists.txt b/devices/virtualAnalogRemapper/CMakeLists.txt index 0f24db4..ec6467d 100644 --- a/devices/virtualAnalogRemapper/CMakeLists.txt +++ b/devices/virtualAnalogRemapper/CMakeLists.txt @@ -2,8 +2,6 @@ # Authors: Silvio Traversaro # CopyPolicy: Released under the terms of the GNU LGPL v2+ -find_package(YARP REQUIRED) - yarp_prepare_plugin(virtualAnalogRemapper CATEGORY device TYPE yarp::dev::VirtualAnalogRemapper INCLUDE "VirtualAnalogRemapper.h" diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt index 674cf88..e4951e3 100644 --- a/devices/wholeBodyDynamics/CMakeLists.txt +++ b/devices/wholeBodyDynamics/CMakeLists.txt @@ -2,10 +2,6 @@ # Authors: Silvio Traversaro # CopyPolicy: Released under the terms of the GNU LGPL v2+ -find_package(YARP REQUIRED) -# ICUB is required for skinDynLib -find_package(ICUB REQUIRED) -find_package(iDynTree REQUIRED) yarp_prepare_plugin(wholebodydynamics CATEGORY device TYPE yarp::dev::WholeBodyDynamicsDevice diff --git a/libraries/ctrlLibRT/CMakeLists.txt b/libraries/ctrlLibRT/CMakeLists.txt index 3c4b0bb..ba72a55 100644 --- a/libraries/ctrlLibRT/CMakeLists.txt +++ b/libraries/ctrlLibRT/CMakeLists.txt @@ -4,8 +4,6 @@ cmake_minimum_required(VERSION 3.5) -find_package(Eigen3 REQUIRED) - project(ctrlLibRT) set(${PROJECT_NAME}_HDRS include/${PROJECT_NAME}/filters.h) From 5531e93b31c22249e0a10004bdcc0eb066cacf9e Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 5 Mar 2024 22:26:53 +0100 Subject: [PATCH 2/3] Replace deprecated iDynTree headers --- devices/baseEstimatorV1/include/Utils.hpp | 8 ++++---- .../baseEstimatorV1/include/baseEstimatorV1.h | 16 ++++++++-------- devices/baseEstimatorV1/src/Utils.cpp | 2 +- .../GravityCompensationHelpers.cpp | 10 +++++----- .../GravityCompensationHelpers.h | 6 +++--- .../SixAxisForceTorqueMeasureHelpers.cpp | 2 +- .../SixAxisForceTorqueMeasureHelpers.h | 4 ++-- .../WholeBodyDynamicsDevice.cpp | 6 +++--- .../wholeBodyDynamics/WholeBodyDynamicsDevice.h | 4 ++-- 9 files changed, 29 insertions(+), 29 deletions(-) diff --git a/devices/baseEstimatorV1/include/Utils.hpp b/devices/baseEstimatorV1/include/Utils.hpp index af328f7..ddd3650 100644 --- a/devices/baseEstimatorV1/include/Utils.hpp +++ b/devices/baseEstimatorV1/include/Utils.hpp @@ -20,10 +20,10 @@ #include // iDynTree -#include -#include -#include -#include +#include +#include +#include +#include // eigen #include diff --git a/devices/baseEstimatorV1/include/baseEstimatorV1.h b/devices/baseEstimatorV1/include/baseEstimatorV1.h index a841596..d8a0eb3 100644 --- a/devices/baseEstimatorV1/include/baseEstimatorV1.h +++ b/devices/baseEstimatorV1/include/baseEstimatorV1.h @@ -9,16 +9,16 @@ #ifndef BASE_ESTIMATOR_V1_H #define BASE_ESTIMATOR_V1_H -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/devices/baseEstimatorV1/src/Utils.cpp b/devices/baseEstimatorV1/src/Utils.cpp index 6f07cbf..86b8885 100644 --- a/devices/baseEstimatorV1/src/Utils.cpp +++ b/devices/baseEstimatorV1/src/Utils.cpp @@ -16,7 +16,7 @@ #include // iDynTree -#include +#include #include iDynTree::Matrix3x3 iDynTreeHelper::Rotation::skewSymmetric(const iDynTree::Matrix3x3& input) diff --git a/devices/wholeBodyDynamics/GravityCompensationHelpers.cpp b/devices/wholeBodyDynamics/GravityCompensationHelpers.cpp index 33a8961..23c8174 100644 --- a/devices/wholeBodyDynamics/GravityCompensationHelpers.cpp +++ b/devices/wholeBodyDynamics/GravityCompensationHelpers.cpp @@ -1,11 +1,11 @@ #include "GravityCompensationHelpers.h" -#include -#include -#include -#include +#include +#include +#include +#include -#include +#include using namespace iDynTree; diff --git a/devices/wholeBodyDynamics/GravityCompensationHelpers.h b/devices/wholeBodyDynamics/GravityCompensationHelpers.h index 8ed62b9..575f777 100644 --- a/devices/wholeBodyDynamics/GravityCompensationHelpers.h +++ b/devices/wholeBodyDynamics/GravityCompensationHelpers.h @@ -2,9 +2,9 @@ #define GRAVITY_COMPENSATION_HELPERS_H // iDynTree includes -#include -#include -#include +#include +#include +#include namespace wholeBodyDynamics diff --git a/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp b/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp index 6ca8f74..a08c5ef 100644 --- a/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp +++ b/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.cpp @@ -1,5 +1,5 @@ #include "SixAxisForceTorqueMeasureHelpers.h" -#include +#include namespace wholeBodyDynamics diff --git a/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.h b/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.h index 0241a92..acfad81 100644 --- a/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.h +++ b/devices/wholeBodyDynamics/SixAxisForceTorqueMeasureHelpers.h @@ -2,8 +2,8 @@ #define SIX_AXIS_FORCE_TORQUE_MEASURE_PROCESSOR_H // iDynTree includes -#include -#include +#include +#include namespace wholeBodyDynamics diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index e3dfa3f..a76c7c4 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -11,9 +11,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h index 32850b3..12837e6 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -19,10 +19,10 @@ #include // iDynTree includes -#include +#include #include #include -#include +#include // Filters #include "ctrlLibRT/filters.h" From 999d45282afe047ce54bf0dd6c1c9abac0a6783f Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 5 Mar 2024 22:51:54 +0100 Subject: [PATCH 3/3] Switch to use iDynTree::Model::sensors() to access the sensors of a model --- .../baseEstimatorV1/include/baseEstimatorV1.h | 1 - .../baseEstimatorV1/src/fbeRobotInterface.cpp | 21 ++++--- .../WholeBodyDynamicsDevice.cpp | 58 +++++++++---------- 3 files changed, 39 insertions(+), 41 deletions(-) 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(); } }