Skip to content

Refactor YarpSensorBridge and remove IMU functionality #1035

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

Merged
GiulioRomualdi merged 2 commits into from
Apr 5, 2026
Merged

Refactor YarpSensorBridge and remove IMU functionality #1035

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
53bdd7a
Refactor YarpSensorBridge implementation to simplify sensor name retr…
GiulioRomualdi Apr 4, 2026
596fb44
Remove Analog sensor IMU-related functionality from SensorBridge and …
GiulioRomualdi Apr 4, 2026
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: 0 additions & 2 deletions bindings/python/RobotInterface/src/ISensorBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@ void CreateISensorBridge(pybind11::module& module)
py::class_<SensorBridgeOptions>(module, "SensorBridgeOptions")
.def(py::init())
.def_readwrite("is_kinematics_enabled", &SensorBridgeOptions::isJointSensorsEnabled)
.def_readwrite("is_imu_enabled", &SensorBridgeOptions::isIMUEnabled)
.def_readwrite("is_linear_accelerometer_enabled",
&SensorBridgeOptions::isLinearAccelerometerEnabled)
.def_readwrite("is_gyroscope_enabled", &SensorBridgeOptions::isGyroscopeEnabled)
Expand All @@ -47,7 +46,6 @@ void CreateISensorBridge(pybind11::module& module)
py::class_<SensorLists>(module, "SensorLists")
.def(py::init())
.def_readwrite("joints_list", &SensorLists::jointsList)
.def_readwrite("imus_list", &SensorLists::IMUsList)
.def_readwrite("linear_accelerometers_list", &SensorLists::linearAccelerometersList)
.def_readwrite("gyroscopes_list", &SensorLists::gyroscopesList)
.def_readwrite("orientation_sensors_list", &SensorLists::orientationSensorsList)
Expand Down
15 changes: 0 additions & 15 deletions bindings/python/RobotInterface/src/YarpSensorBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,12 +60,6 @@ void CreateYarpSensorBridge(pybind11::module& module)
bool ok = impl.getJointsList(list);
return std::make_tuple(ok, list);
})
.def("get_imus_list",
[](YarpSensorBridge& impl) {
std::vector<std::string> list;
bool ok = impl.getIMUsList(list);
return std::make_tuple(ok, list);
})
.def("get_gyroscopes_list",
[](YarpSensorBridge& impl) {
std::vector<std::string> list;
Expand Down Expand Up @@ -135,15 +129,6 @@ void CreateYarpSensorBridge(pybind11::module& module)
bool ok = impl.getJointAccelerations(joints, receiveTimeInSeconds);
return std::make_tuple(ok, joints, receiveTimeInSeconds);
})
.def(
"get_imu_measurement",
[](YarpSensorBridge& impl, const std::string& name) {
Eigen::Matrix<double, 12, 1> measurement;
double receiveTimeInSeconds;
bool ok = impl.getIMUMeasurement(name, measurement, receiveTimeInSeconds);
return std::make_tuple(ok, measurement, receiveTimeInSeconds);
},
py::arg("sensor_name"))
.def(
"get_linear_accelerometer_measurement",
[](YarpSensorBridge& impl, const std::string& name) {
Expand Down
10 changes: 5 additions & 5 deletions devices/FloatingBaseEstimatorDevice/src/FloatingBaseEstimatorDevice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -298,15 +298,15 @@ bool FloatingBaseEstimatorDevice::updateMeasurements()

bool FloatingBaseEstimatorDevice::updateInertialBuffers()
{
Eigen::Matrix<double, 12, 1> imuMeasure;
if (!m_robotSensorBridge->getIMUMeasurement(m_baseLinkImuName, imuMeasure))
Eigen::Vector3d accMeasure;
Eigen::Vector3d gyroMeasure;
if (!m_robotSensorBridge->getLinearAccelerometerMeasurement(m_baseLinkImuName, accMeasure)
|| !m_robotSensorBridge->getGyroscopeMeasure(m_baseLinkImuName, gyroMeasure))
{
return false;
}

const int accOffset{3};
const int gyroOffset{6};
if (!m_estimator->setIMUMeasurement(imuMeasure.segment<3>(accOffset), imuMeasure.segment<3>(gyroOffset)))
if (!m_estimator->setIMUMeasurement(accMeasure, gyroMeasure))
{
return false;
}
Expand Down
6 changes: 0 additions & 6 deletions devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
using accelerometer_t = Eigen::Matrix<double, 3, 1>;
using orientation_t = Eigen::Matrix<double, 3, 1>;
using magnemetometer_t = Eigen::Matrix<double, 3, 1>;
using analog_sensor_t = Eigen::Matrix<double, 12, 1>;

std::unique_ptr<BipedalLocomotion::RobotInterface::YarpSensorBridge> m_robotSensorBridge;
std::unique_ptr<BipedalLocomotion::RobotInterface::YarpCameraBridge> m_cameraBridge;
Expand Down Expand Up @@ -207,7 +206,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
accelerometer_t m_acceloremeterBuffer;
orientation_t m_orientationBuffer;
magnemetometer_t m_magnemetometerBuffer;
analog_sensor_t m_analogSensorBuffer;
double m_ftTemperatureBuffer;

bool m_streamMotorStates{false};
Expand Down Expand Up @@ -257,10 +255,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
VideoWriter& writer,
ExogenousSignal<yarp::sig::ImageOf<yarp::sig::PixelRgb>>& signal);
void saveCodeStatus(const std::string& logPrefix, const std::string& fileName) const;
void unpackIMU(Eigen::Ref<const analog_sensor_t> signal,
Eigen::Ref<accelerometer_t> accelerometer,
Eigen::Ref<gyro_t> gyro,
Eigen::Ref<orientation_t> orientation);
bool setupRobotSensorBridge(std::weak_ptr<const ParametersHandler::IParametersHandler> params);
bool setupRobotCameraBridge(std::weak_ptr<const ParametersHandler::IParametersHandler> params);
bool setupTelemetry(std::weak_ptr<const ParametersHandler::IParametersHandler> params,
Expand Down
54 changes: 0 additions & 54 deletions devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1302,22 +1302,6 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::prepareRobotLogging()
magnetometerElementNames);
}

// an IMU contains a gyro accelerometer and an orientation sensor
for (const auto& sensorName : m_robotSensorBridge->getIMUsList())
{
std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName;
std::string fullGyroSensorName = gyrosName + treeDelim + sensorName;
std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName;
ok = ok
&& addChannel(fullAccelerometerSensorName,
accelerometerElementNames.size(), //
accelerometerElementNames);
ok = ok && addChannel(fullGyroSensorName, gyroElementNames.size(), gyroElementNames);
ok = ok
&& addChannel(fullOrientationsSensorName,
orientationElementNames.size(), //
orientationElementNames);
}
}

if (m_streamCartesianWrenches)
Expand Down Expand Up @@ -1633,22 +1617,6 @@ bool YarpRobotLoggerDevice::createFramesFolder(std::shared_ptr<VideoWriter::Imag
return true;
}

void YarpRobotLoggerDevice::unpackIMU(Eigen::Ref<const analog_sensor_t> signal,
Eigen::Ref<accelerometer_t> accelerometer,
Eigen::Ref<gyro_t> gyro,
Eigen::Ref<orientation_t> orientation)
{
// the output consists 12 double, organized as follows:
// euler angles [3]
// linear acceleration [3]
// angular speed [3]
// magnetic field [3]
// http://wiki.icub.org/wiki/Inertial_Sensor
orientation = signal.segment<3>(0);
accelerometer = signal.segment<3>(3);
gyro = signal.segment<3>(6);
}

void YarpRobotLoggerDevice::lookForExogenousSignals()
{
yarp::profiler::NetworkProfiler::ports_name_set yarpPorts;
Expand Down Expand Up @@ -2303,28 +2271,6 @@ void YarpRobotLoggerDevice::run()
}
}

// an IMU contains a gyro accelerometer and an orientation sensor
for (const auto& sensorName : m_robotSensorBridge->getIMUsList())
{
if (m_robotSensorBridge->getIMUMeasurement(sensorName, m_analogSensorBuffer))
{
// it will return a tuple containing the Accelerometer, the gyro and the
// orientation
this->unpackIMU(m_analogSensorBuffer,
m_acceloremeterBuffer,
m_gyroBuffer,
m_orientationBuffer);

signalFullName = accelerometersName + treeDelim + sensorName;
logData(signalFullName, m_acceloremeterBuffer, time);

signalFullName = gyrosName + treeDelim + sensorName;
logData(signalFullName, m_gyroBuffer, time);

signalFullName = orientationsName + treeDelim + sensorName;
logData(signalFullName, m_orientationBuffer, time);
}
}
}

if (m_streamCartesianWrenches)
Expand Down
49 changes: 5 additions & 44 deletions ...tInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,9 @@ namespace RobotInterface
* YarpSensorBridge Yarp implementation of the ISensorBridge interface
* Currently available interfaces
* - Remapped Remote Control Board for joint states
* - Inertial Measurement Units through generic sensor interface and remapped multiple analog sensor interface
* - MAS-based inertials (gyroscopes, accelerometers, orientation sensors, magnetometers)
* - Whole Body Dynamics Estimated end effector wrenches through a generic sensor interface
* - Force Torque Sensors through analog sensor interface and remapped multiple analog sensor interface
* - Force Torque Sensors through remapped multiple analog sensor interface
* - Depth Cameras through RGBD sensor interface
* - Camera images through OpenCV Grabber interface
*
Expand Down Expand Up @@ -64,16 +64,15 @@ namespace RobotInterface
* | |stream_motor_temperature | boolean |Flag to activate the attachment to motor temperature sensors |
* |RemoteControlBoardRemapper | | |Expects only one remapped remotecontrolboard device attached to it, if there multiple remote control boards, then use a remapper to create a single remotecontrolboard |
* | |joints_list | vector of strings |This parameter is **optional**. The joints list used to open the remote control board remapper. If the list is not passed, the order of the joint stored in the PolyDriver is used |
* |InertialSensors | | |Expects IMU to be opened as genericSensorClient devices communicating through the inertial server and other inertials as a part multiple analog sensors remapper ("multipleanalogsensorsremapper") |
* | |imu_list | vector of strings |list of the names of devices opened as genericSensorClient device and having a channel dimension of 12 |
* |InertialSensors | | |Expects inertials to be opened as a part of the multiple analog sensors remapper ("multipleanalogsensorsremapper") |
* | |gyroscopes_list | vector of strings |list of the names of devices opened with ThreeAxisGyroscope interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 |
* | |accelerometers_list | vector of strings |list of the names of devices opened with ThreeAxisLinearAccelerometers interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 |
* | |orientation_sensors_list | vector of strings |list of the names of devices opened with OrientationSensors interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 |
* | |magnetometers_list | vector of strings |list of the names of devices opened with ThreeAxisMagnetometer interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 |
* |CartesianWrenches | | |Expects the devices wrapping the cartesian wrenches ports to be opened as "genericSensorClient" device and have a channel dimension of 6 |
* | |cartesian_wrenches_list | vector of strings |list of the names of devices opened as genericSensorClient device and having a channel dimension of 6 |
* |SixAxisForceTorqueSensors | | |Expects the Six axis FT sensors to be opened with SixAxisForceTorqueSensors interface remapped through multiple analog sensors remapper ("multipleanalogsensorsremapper") or to be opened as analog sensor ("analogsensorclient") device having channel dimensions as 6|
* | |sixaxis_forcetorque_sensors_list | vector of strings |list of six axis FT sensors (the difference between a MAS FT and an analog FT is done internally assuming that the names are distinct form each other)|
* |SixAxisForceTorqueSensors | | |Expects the Six axis FT sensors to be opened with SixAxisForceTorqueSensors interface remapped through multiple analog sensors remapper ("multipleanalogsensorsremapper")|
* | |sixaxis_forcetorque_sensors_list | vector of strings |list of six axis FT sensors |
* |TemperatureSensors | | |Expects the temperature sensors to be opened with TemperatureSensors interface remapped through multiple analog sensors remapper|
* | |temperature_sensors_list | vector of strings |list containing the devices opened with TemperatureSensors interface |
*/
Expand Down Expand Up @@ -137,18 +136,6 @@ class YarpSensorBridge : public ISensorBridge,
*/
bool getJointsList(std::vector<std::string>& jointsList) final;

/**
* Get imu sensors
* @param[out] IMUsList list of IMUs attached to the bridge
* @return true/false in case of success/failure
*/
bool getIMUsList(std::vector<std::string>& IMUsList) final;

/**
* Get linear accelerometers
* @param[out] linearAccelerometersList list of linear accelerometers attached to the bridge
* @return true/false in case of success/failure
*/
bool getLinearAccelerometersList(std::vector<std::string>& linearAccelerometersList) final;

/**
Expand Down Expand Up @@ -206,8 +193,6 @@ class YarpSensorBridge : public ISensorBridge,

const std::vector<std::string>& getJointsList() const;

const std::vector<std::string>& getIMUsList() const;

const std::vector<std::string>& getLinearAccelerometersList() const;

const std::vector<std::string>& getGyroscopesList() const;
Expand Down Expand Up @@ -291,30 +276,6 @@ class YarpSensorBridge : public ISensorBridge,
bool getJointAccelerations(Eigen::Ref<Eigen::VectorXd> jointAccelerations,
OptionalDoubleRef receiveTimeInSeconds = {}) final;

/**
* Get IMU measurement
* The serialization of measurments is as follows,
* (rpy acc omega mag)
* - rpy in radians Roll-Pitch-Yaw Euler angles
* - acc in m/s^2 linear accelerometer measurements
* - omega in rad/s gyroscope measurements
* - mag in tesla magnetometer measurements
* @param[in] imuName name of the IMU
* @param[out] imuMeasurement imu measurement of size 12
* @param[out] receiveTimeInSeconds time at which the measurement was received
* @return true/false in case of success/failure
*/
bool getIMUMeasurement(const std::string& imuName,
Eigen::Ref<Vector12d> imuMeasurement,
OptionalDoubleRef receiveTimeInSeconds = {}) final;

/**
* Get linear accelerometer measurement in m/s^2
* @param[in] accName name of the linear accelerometer
* @param[out] accMeasurement linear accelerometer measurements of size 3
* @param[out] receiveTimeInSeconds time at which the measurement was received
* @return true/false in case of success/failure
*/
bool getLinearAccelerometerMeasurement(const std::string& accName,
Eigen::Ref<Eigen::Vector3d> accMeasurement,
OptionalDoubleRef receiveTimeInSeconds = {}) final;
Expand Down
Loading
Loading