diff --git a/include/sdf/Imu.hh b/include/sdf/Imu.hh
index 0fd1fbde3..ed261483d 100644
--- a/include/sdf/Imu.hh
+++ b/include/sdf/Imu.hh
@@ -109,6 +109,36 @@ namespace sdf
/// \param[in] _noise Noise values for the Z-axis angular velocity.
public: void SetAngularVelocityZNoise(const Noise &_noise);
+ /// \brief Get the noise values related to the body-frame orientation
+ /// on the X-axis (Roll).
+ /// \return Noise values for the X-axis orientation.
+ public: const Noise &OrientationXNoise() const;
+
+ /// \brief Set the noise values related to the body-frame orientation
+ /// on the X-axis (Roll).
+ /// \param[in] _noise Noise values for the X-axis orientation.
+ public: void SetOrientationXNoise(const Noise &_noise) ;
+
+ /// \brief Get the noise values related to the body-frame orientation
+ /// on the Y-axis (Pitch).
+ /// \return Noise values for the Y-axis orientation.
+ public: const Noise &OrientationYNoise() const;
+
+ /// \brief Set the noise values related to the body-frame orientation
+ /// on the Y-axis (Pitch).
+ /// \param[in] _noise Noise values for the Y-axis orientation.
+ public: void SetOrientationYNoise(const Noise &_noise) ;
+
+ /// \brief Get the noise values related to the body-frame orientation
+ /// on the Z-axis (Yaw).
+ /// \return Noise values for the Z-axis orientation.
+ public: const Noise &OrientationZNoise() const;
+
+ /// \brief Set the noise values related to the body-frame orientation
+ /// on the Z-axis (Yaw).
+ /// \param[in] _noise Noise values for the Z-axis orientation.
+ public: void SetOrientationZNoise(const Noise &_noise) ;
+
/// \brief Used when localization is set to GRAV_UP or GRAV_DOWN, a
/// projection of this vector into a plane that is orthogonal to the
/// gravity vector defines the direction of the IMU reference frame's
diff --git a/sdf/1.12/imu.sdf b/sdf/1.12/imu.sdf
index 25690c8f1..ba7f6f691 100644
--- a/sdf/1.12/imu.sdf
+++ b/sdf/1.12/imu.sdf
@@ -117,4 +117,22 @@
Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation.
+
+
+ These elements are specific to body-frame orientation,
+ which is expressed in radians
+
+ Orientation about X-axis (Roll)
+
+
+
+ Orientation about Y-axis (Pitch)
+
+
+
+ Orientation about Z-axis (Yaw)
+
+
+
+
diff --git a/src/Imu.cc b/src/Imu.cc
index 0f6b9870a..8685f9b60 100644
--- a/src/Imu.cc
+++ b/src/Imu.cc
@@ -47,6 +47,18 @@ class sdf::Imu::Implementation
/// Z-axis.
public: Noise angularVelZNoise;
+ /// \brief Noise values related to the body-frame orientation on the
+ /// X-axis (Roll).
+ public: Noise orientationXNoise;
+
+ /// \brief Noise values related to the body-frame orientation on the
+ /// Y-axis (Pitch).
+ public: Noise orientationYNoise;
+
+ /// \brief Noise values related to the body-frame orientation on the
+ /// Z-axis (Yaw).
+ public: Noise orientationZNoise;
+
/// \brief The gravity dir
public: gz::math::Vector3d gravityDirX{gz::math::Vector3d::UnitX};
@@ -195,6 +207,38 @@ Errors Imu::Load(ElementPtr _sdf)
errors, "enable_orientation", this->dataPtr->orientationEnabled).first;
}
+ if (_sdf->HasElement("orientation"))
+ {
+ sdf::ElementPtr elem = _sdf->GetElement("orientation", errors);
+ if (elem->HasElement("x"))
+ {
+ if (elem->GetElement("x", errors)->HasElement("noise"))
+ {
+ sdf::Errors loadErrors = this->dataPtr->orientationXNoise.Load(
+ elem->GetElement("x", errors)->GetElement("noise", errors));
+ errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
+ }
+ }
+ if (elem->HasElement("y"))
+ {
+ if (elem->GetElement("y", errors)->HasElement("noise"))
+ {
+ sdf::Errors loadErrors = this->dataPtr->orientationYNoise.Load(
+ elem->GetElement("y", errors)->GetElement("noise", errors));
+ errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
+ }
+ }
+ if (elem->HasElement("z"))
+ {
+ if (elem->GetElement("z", errors)->HasElement("noise"))
+ {
+ sdf::Errors loadErrors = this->dataPtr->orientationZNoise.Load(
+ elem->GetElement("z", errors)->GetElement("noise", errors));
+ errors.insert(errors.end(), loadErrors.begin(), loadErrors.end());
+ }
+ }
+ }
+
return errors;
}
@@ -219,6 +263,9 @@ bool Imu::operator==(const Imu &_imu) const
this->dataPtr->angularVelXNoise == _imu.dataPtr->angularVelXNoise &&
this->dataPtr->angularVelYNoise == _imu.dataPtr->angularVelYNoise &&
this->dataPtr->angularVelZNoise == _imu.dataPtr->angularVelZNoise &&
+ this->dataPtr->orientationXNoise == _imu.dataPtr->orientationXNoise &&
+ this->dataPtr->orientationYNoise == _imu.dataPtr->orientationYNoise &&
+ this->dataPtr->orientationZNoise == _imu.dataPtr->orientationZNoise &&
this->dataPtr->localization == _imu.dataPtr->localization &&
this->dataPtr->gravityDirX == _imu.dataPtr->gravityDirX &&
@@ -302,6 +349,42 @@ void Imu::SetAngularVelocityZNoise(const Noise &_noise)
this->dataPtr->angularVelZNoise = _noise;
}
+//////////////////////////////////////////////////
+const Noise &Imu::OrientationXNoise() const
+{
+ return this->dataPtr->orientationXNoise;
+}
+
+//////////////////////////////////////////////////
+void Imu::SetOrientationXNoise(const Noise &_noise)
+{
+ this->dataPtr->orientationXNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &Imu::OrientationYNoise() const
+{
+ return this->dataPtr->orientationYNoise;
+}
+
+//////////////////////////////////////////////////
+void Imu::SetOrientationYNoise(const Noise &_noise)
+{
+ this->dataPtr->orientationYNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &Imu::OrientationZNoise() const
+{
+ return this->dataPtr->orientationZNoise;
+}
+
+//////////////////////////////////////////////////
+void Imu::SetOrientationZNoise(const Noise &_noise)
+{
+ this->dataPtr->orientationZNoise = _noise;
+}
+
//////////////////////////////////////////////////
const gz::math::Vector3d &Imu::GravityDirX() const
{
@@ -447,6 +530,26 @@ sdf::ElementPtr Imu::ToElement(sdf::Errors &_errors) const
linearAccZNoiseElem->Copy(this->dataPtr->linearAccelZNoise.ToElement(
_errors), _errors);
+ sdf::ElementPtr orientationElem = elem->GetElement(
+ "orientation", _errors);
+ sdf::ElementPtr orientationXElem = orientationElem->GetElement("x", _errors);
+ sdf::ElementPtr orientationXNoiseElem = orientationXElem->GetElement(
+ "noise", _errors);
+ orientationXNoiseElem->Copy(this->dataPtr->orientationXNoise.ToElement(
+ _errors), _errors);
+
+ sdf::ElementPtr orientationYElem = orientationElem->GetElement("y", _errors);
+ sdf::ElementPtr orientationYNoiseElem = orientationYElem->GetElement(
+ "noise", _errors);
+ orientationYNoiseElem->Copy(this->dataPtr->orientationYNoise.ToElement(
+ _errors), _errors);
+
+ sdf::ElementPtr orientationZElem = orientationElem->GetElement("z", _errors);
+ sdf::ElementPtr orientationZNoiseElem = orientationZElem->GetElement(
+ "noise", _errors);
+ orientationZNoiseElem->Copy(this->dataPtr->orientationZNoise.ToElement(
+ _errors), _errors);
+
elem->GetElement("enable_orientation", _errors)->Set(
_errors, this->OrientationEnabled());
diff --git a/src/Imu_TEST.cc b/src/Imu_TEST.cc
index 3c9cd5682..5f2a845a7 100644
--- a/src/Imu_TEST.cc
+++ b/src/Imu_TEST.cc
@@ -56,6 +56,18 @@ TEST(DOMImu, Construction)
imu.SetAngularVelocityZNoise(noise);
EXPECT_EQ(noise, imu.AngularVelocityZNoise());
+ EXPECT_EQ(defaultNoise, imu.OrientationXNoise());
+ imu.SetOrientationXNoise(noise);
+ EXPECT_EQ(noise, imu.OrientationXNoise());
+
+ EXPECT_EQ(defaultNoise, imu.OrientationYNoise());
+ imu.SetOrientationYNoise(noise);
+ EXPECT_EQ(noise, imu.OrientationYNoise());
+
+ EXPECT_EQ(defaultNoise, imu.OrientationZNoise());
+ imu.SetOrientationZNoise(noise);
+ EXPECT_EQ(noise, imu.OrientationZNoise());
+
EXPECT_EQ(gz::math::Vector3d::UnitX, imu.GravityDirX());
imu.SetGravityDirX(gz::math::Vector3d::Zero);
EXPECT_EQ(gz::math::Vector3d::Zero, imu.GravityDirX());
@@ -146,6 +158,9 @@ TEST(DOMImu, ToElement)
imu.SetAngularVelocityXNoise(noise);
imu.SetAngularVelocityYNoise(noise);
imu.SetAngularVelocityZNoise(noise);
+ imu.SetOrientationXNoise(noise);
+ imu.SetOrientationYNoise(noise);
+ imu.SetOrientationZNoise(noise);
imu.SetGravityDirX(gz::math::Vector3d::Zero);
imu.SetGravityDirXParentFrame("my_frame");
imu.SetCustomRpy(gz::math::Vector3d::UnitZ);
@@ -167,6 +182,9 @@ TEST(DOMImu, ToElement)
EXPECT_EQ(noise, imu2.AngularVelocityXNoise());
EXPECT_EQ(noise, imu2.AngularVelocityYNoise());
EXPECT_EQ(noise, imu2.AngularVelocityZNoise());
+ EXPECT_EQ(noise, imu2.OrientationXNoise());
+ EXPECT_EQ(noise, imu2.OrientationYNoise());
+ EXPECT_EQ(noise, imu2.OrientationZNoise());
EXPECT_EQ(gz::math::Vector3d::Zero, imu2.GravityDirX());
EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame());
EXPECT_EQ(gz::math::Vector3d::UnitZ, imu2.CustomRpy());
@@ -215,6 +233,9 @@ TEST(DOMImu, ToElementErrorOutput)
imu.SetAngularVelocityXNoise(noise);
imu.SetAngularVelocityYNoise(noise);
imu.SetAngularVelocityZNoise(noise);
+ imu.SetOrientationXNoise(noise);
+ imu.SetOrientationYNoise(noise);
+ imu.SetOrientationZNoise(noise);
imu.SetGravityDirX(gz::math::Vector3d::Zero);
imu.SetGravityDirXParentFrame("my_frame");
imu.SetCustomRpy(gz::math::Vector3d::UnitZ);
@@ -238,6 +259,9 @@ TEST(DOMImu, ToElementErrorOutput)
EXPECT_EQ(noise, imu2.AngularVelocityXNoise());
EXPECT_EQ(noise, imu2.AngularVelocityYNoise());
EXPECT_EQ(noise, imu2.AngularVelocityZNoise());
+ EXPECT_EQ(noise, imu2.OrientationXNoise());
+ EXPECT_EQ(noise, imu2.OrientationYNoise());
+ EXPECT_EQ(noise, imu2.OrientationZNoise());
EXPECT_EQ(gz::math::Vector3d::Zero, imu2.GravityDirX());
EXPECT_EQ("my_frame", imu2.GravityDirXParentFrame());
EXPECT_EQ(gz::math::Vector3d::UnitZ, imu2.CustomRpy());