Reference initial orientation to world frame in IMU plugin

CHANGELOG.md

@@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### 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).
+
 ## [4.5.1] - 2022-08-31
 
 ### Fixed

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)) {

plugins/imu/src/IMUDriver.cpp

@@ -14,6 +14,8 @@
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
 
+#include <ignition/math/Quaternion.hh>
+
 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));