From 6e94671575c60d86f6d24c615ca97830e4805537 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 4 Nov 2022 23:05:48 +0100 Subject: [PATCH 1/2] Reference initial orientation to world frame in IMU plugin --- CHANGELOG.md | 6 +++++- plugins/imu/src/IMU.cc | 3 +++ plugins/imu/src/IMUDriver.cpp | 6 ++++++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f2d73ae41..971cbb8a4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,11 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] -### Fixed +### Added + +- Added a new `useWorldReferenceOrientation` config option in the `gazebo_imu` plugin to take account for any initial non-zero orientation of the sensor as measured via `yarp::dev::IOrientationSensors` (https://github.com/robotology/gazebo-yarp-plugins/pull/639). + +### Fixed - Fix wrong install include for gazebo_yarp_lib_common library (https://github.com/robotology/gazebo-yarp-plugins/pull/644). diff --git a/plugins/imu/src/IMU.cc b/plugins/imu/src/IMU.cc index 90ee91256..52f18c402 100644 --- a/plugins/imu/src/IMU.cc +++ b/plugins/imu/src/IMU.cc @@ -152,6 +152,9 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) {"sensor_name", ::yarp::os::Value{_sensor->Name()}} }; + if (m_parameters.check("useWorldReferenceOrientation")) { + imu_properties.put("useWorldReferenceOrientation", yarp::os::Value::getNullValue()); + } //Open the driver if (!m_imuDriver.open(imu_properties)) { diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp index 8c0e29b4e..cbdc868d0 100644 --- a/plugins/imu/src/IMUDriver.cpp +++ b/plugins/imu/src/IMUDriver.cpp @@ -14,6 +14,8 @@ #include #include +#include + using namespace boost::placeholders; using namespace yarp::dev; @@ -88,6 +90,10 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config) return false; } + if (config.check("useWorldReferenceOrientation")) { + m_parentSensor->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity); + } + //Connect the driver to the gazebo simulation using namespace boost::placeholders; this->m_updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpIMUDriver::onUpdate, this, _1)); From 1559f552ce87339befb8a3e4e26bdaa3f056b918 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 8 Mar 2023 20:01:05 +0100 Subject: [PATCH 2/2] Default to new behavior, make the old one opt-in --- CHANGELOG.md | 4 ++-- plugins/imu/src/IMU.cc | 4 ++-- plugins/imu/src/IMUDriver.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 971cbb8a4..6be955b86 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,9 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] -### Added +### Changed -- Added a new `useWorldReferenceOrientation` config option in the `gazebo_imu` plugin to take account for any initial non-zero orientation of the sensor as measured via `yarp::dev::IOrientationSensors` (https://github.com/robotology/gazebo-yarp-plugins/pull/639). +- The `gazebo_imu` plugin used to output orientation measurements as if the sensor was zero-aligned with the world frame, regardless of the initial orientation of the part the sensor is attached to. To match most real-world IMUs, the default behavior was changed to allow measuring orientation with regard to the world frame at all times. The old behavior can be restored by setting the `useInitialSensorOrientationAsReference` config option (https://github.com/robotology/gazebo-yarp-plugins/pull/639). ### Fixed diff --git a/plugins/imu/src/IMU.cc b/plugins/imu/src/IMU.cc index 52f18c402..6ae4b856c 100644 --- a/plugins/imu/src/IMU.cc +++ b/plugins/imu/src/IMU.cc @@ -152,8 +152,8 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) {"sensor_name", ::yarp::os::Value{_sensor->Name()}} }; - if (m_parameters.check("useWorldReferenceOrientation")) { - imu_properties.put("useWorldReferenceOrientation", yarp::os::Value::getNullValue()); + if (m_parameters.check("useInitialSensorOrientationAsReference")) { + imu_properties.put("useInitialSensorOrientationAsReference", yarp::os::Value::getNullValue()); } //Open the driver diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp index cbdc868d0..5248d7dc1 100644 --- a/plugins/imu/src/IMUDriver.cpp +++ b/plugins/imu/src/IMUDriver.cpp @@ -90,7 +90,7 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config) return false; } - if (config.check("useWorldReferenceOrientation")) { + if (!config.check("useInitialSensorOrientationAsReference")) { m_parentSensor->SetWorldToReferenceOrientation(ignition::math::Quaterniond::Identity); }