Reference initial orientation to world frame in IMU plugin

CHANGELOG.md

@@ -5,7 +5,11 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
-### Fixed 
+### Changed
+
+- 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
 
 - Fix wrong install include for gazebo_yarp_lib_common library (https://github.com/robotology/gazebo-yarp-plugins/pull/644).
 

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("useInitialSensorOrientationAsReference")) {
+        imu_properties.put("useInitialSensorOrientationAsReference", 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("useInitialSensorOrientationAsReference")) {
+        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));