Added Force Torque Noise functions + Unit tests

include/sdf/ForceTorque.hh

@@ -79,6 +79,54 @@ namespace sdf
     /// not been called.
     public: sdf::ElementPtr Element() const;
 
+    /// \brief Get the force noise values in the measurement frame X-axis.
+    /// \return Noise values for the X-axis force.
+    public: const Noise &ForceXNoise() const;
+
+    /// \brief Set the force noise values in the measurement frame X-axis.
+    /// \param[in] _noise Noise values for the X-axis force.
+    public: void SetForceXNoise(const Noise &_noise);
+
+    /// \brief Get the force noise values in the measurement frame Y-axis.
+    /// \return Noise values for the Y-axis force.
+    public: const Noise &ForceYNoise() const;
+
+    /// \brief Set the force noise values in the measurement frame Y-axis.
+    /// \param[in] _noise Noise values for the Y-axis force.
+    public: void SetForceYNoise(const Noise &_noise);
+
+    /// \brief Get the force noise values in the measurement frame Z-axis.
+    /// \return Noise values for the Z-axis force.
+    public: const Noise &ForceZNoise() const;
+
+    /// \brief Set the force noise values in the measurement frame Z-axis.
+    /// \param[in] _noise Noise values for the Z-axis force.
+    public: void SetForceZNoise(const Noise &_noise);
+
+    /// \brief Get the torque noise values in the measurement frame X-axis.
+    /// \return Noise values for the X-axis torque.
+    public: const Noise &TorqueXNoise() const;
+
+    /// \brief Set the torque noise values in the measurement frame X-axis.
+    /// \param[in] _noise Noise values for the X-axis torque.
+    public: void SetTorqueXNoise(const Noise &_noise);
+
+    /// \brief Get the torque noise values in the measurement frame Y-axis.
+    /// \return Noise values for the Y-axis torque.
+    public: const Noise &TorqueYNoise() const;
+
+    /// \brief Set the torque noise values in the measurement frame Y-axis.
+    /// \param[in] _noise Noise values for the Y-axis torque.
+    public: void SetTorqueYNoise(const Noise &_noise);
+
+    /// \brief Get the torque noise values in the measurement frame Z-axis.
+    /// \return Noise values for the Z-axis torque.
+    public: const Noise &TorqueZNoise() const;
+
+    /// \brief Set the torque noise values in the measurement frame Z-axis.
+    /// \param[in] _noise Noise values for the Z-axis torque.
+    public: void SetTorqueZNoise(const Noise &_noise);
+
     /// \brief Get the frame in which the wrench values are reported.
     /// \return The frame of the wrench values.
     public: ForceTorqueFrame Frame() const;

sdf/1.9/forcetorque.sdf

@@ -17,4 +17,38 @@
         "child_to_parent" if the measured wrench is the one applied by the child link on the parent link.
     </description>
   </element>
+
+  <element name="force" required="0">
+    <description>These elements are specific to measurement-frame force, 
+    which is expressed in Newtons</description>
+    <element name="x" required="0">
+      <description>Force along the X axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="y" required="0">
+      <description>Force along the Y axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="z" required="0">
+      <description>Force along the Z axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+  </element>
+
+  <element name="torque" required="0">
+    <description>These elements are specific to measurement-frame torque, 
+    which is expressed in Newton-meters</description>
+    <element name="x" required="0">
+      <description>Torque about the X axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="y" required="0">
+      <description>Force about the Y axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+    <element name="z" required="0">
+      <description>Torque about the Z axis</description>
+      <include filename="noise.sdf" required="0"/>
+    </element>
+  </element>
 </element>

src/ForceTorque.cc

@@ -22,6 +22,30 @@ using namespace sdf;
 /// \brief Private force torque data.
 class sdf::ForceTorque::Implementation
 {
+  /// \brief Noise values related to the body-frame force on the
+  /// X-axis.
+  public: Noise forceXNoise;
+
+  /// \brief Noise values related to the body-frame force on the
+  /// Y-axis.
+  public: Noise forceYNoise;
+
+  /// \brief Noise values related to the body-frame force on the
+  /// Z-axis.
+  public: Noise forceZNoise;
+
+  /// \brief Noise values related to the body-frame torque on the
+  /// X-axis.
+  public: Noise torqueXNoise;
+
+  /// \brief Noise values related to the body-frame torque on the
+  /// Y-axis.
+  public: Noise torqueYNoise;
+
+  /// \brief Noise values related to the body-frame torque on the
+  /// Z-axis.
+  public: Noise torqueZNoise;
+
   /// \brief Name of the reference frame for the wrench values.
   public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD;
 
@@ -106,6 +130,29 @@ Errors ForceTorque::Load(ElementPtr _sdf)
     }
   }
 
+  auto loadAxisNoise = [&errors](sdf::ElementPtr _parent,
+                          const std::string _groupLabel,
+                          const std::string _axisLabel,
+                          sdf::Noise& _noise)
+  {
+    if (_parent->HasElement(_groupLabel) &&
+        _parent->GetElement(_groupLabel)->HasElement(_axisLabel))
+    {
+        auto axis = _parent->GetElement(_groupLabel)->GetElement(_axisLabel);
+        sdf::Errors noiseErrors = _noise.Load(axis->GetElement("noise"));
+        errors.insert(errors.end(), noiseErrors.begin(), noiseErrors.end());
+        return true;
+    }
+    return false;
+  };
+
+  loadAxisNoise(_sdf, "force", "x", this->dataPtr->forceXNoise);
+  loadAxisNoise(_sdf, "force", "y", this->dataPtr->forceYNoise);
+  loadAxisNoise(_sdf, "force", "z", this->dataPtr->forceZNoise);
+  loadAxisNoise(_sdf, "torque", "x", this->dataPtr->torqueXNoise);
+  loadAxisNoise(_sdf, "torque", "y", this->dataPtr->torqueYNoise);
+  loadAxisNoise(_sdf, "torque", "z", this->dataPtr->torqueZNoise);
+
   return errors;
 }
 
@@ -125,7 +172,85 @@ bool ForceTorque::operator!=(const ForceTorque &_ft) const
 bool ForceTorque::operator==(const ForceTorque &_ft) const
 {
   return this->dataPtr->frame == _ft.dataPtr->frame &&
-         this->dataPtr->measure_direction == _ft.dataPtr->measure_direction;
+         this->dataPtr->measure_direction == _ft.dataPtr->measure_direction &&
+         this->dataPtr->forceXNoise == _ft.dataPtr->forceXNoise &&
+         this->dataPtr->forceYNoise == _ft.dataPtr->forceYNoise &&
+         this->dataPtr->forceZNoise == _ft.dataPtr->forceZNoise &&
+         this->dataPtr->torqueXNoise == _ft.dataPtr->torqueXNoise &&
+         this->dataPtr->torqueYNoise == _ft.dataPtr->torqueYNoise &&
+         this->dataPtr->torqueZNoise == _ft.dataPtr->torqueZNoise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::ForceXNoise() const
+{
+  return this->dataPtr->forceXNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetForceXNoise(const Noise &_noise)
+{
+  this->dataPtr->forceXNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::ForceYNoise() const
+{
+  return this->dataPtr->forceYNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetForceYNoise(const Noise &_noise)
+{
+  this->dataPtr->forceYNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::ForceZNoise() const
+{
+  return this->dataPtr->forceZNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetForceZNoise(const Noise &_noise)
+{
+  this->dataPtr->forceZNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::TorqueXNoise() const
+{
+  return this->dataPtr->torqueXNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetTorqueXNoise(const Noise &_noise)
+{
+  this->dataPtr->torqueXNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::TorqueYNoise() const
+{
+  return this->dataPtr->torqueYNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetTorqueYNoise(const Noise &_noise)
+{
+  this->dataPtr->torqueYNoise = _noise;
+}
+
+//////////////////////////////////////////////////
+const Noise &ForceTorque::TorqueZNoise() const
+{
+  return this->dataPtr->torqueZNoise;
+}
+
+//////////////////////////////////////////////////
+void ForceTorque::SetTorqueZNoise(const Noise &_noise)
+{
+  this->dataPtr->torqueZNoise = _noise;
 }
 
 //////////////////////////////////////////////////

src/ForceTorque_TEST.cc

@@ -22,6 +22,38 @@
 TEST(DOMForceTorque, Construction)
 {
   sdf::ForceTorque ft;
+  sdf::Noise defaultNoise, noise;
+
+  noise.SetType(sdf::NoiseType::GAUSSIAN);
+  noise.SetMean(1.2);
+  noise.SetStdDev(2.3);
+  noise.SetBiasMean(4.5);
+  noise.SetBiasStdDev(6.7);
+  noise.SetPrecision(8.9);
+
+  EXPECT_EQ(defaultNoise, ft.ForceXNoise());
+  ft.SetForceXNoise(noise);
+  EXPECT_EQ(noise, ft.ForceXNoise());
+
+  EXPECT_EQ(defaultNoise, ft.ForceYNoise());
+  ft.SetForceYNoise(noise);
+  EXPECT_EQ(noise, ft.ForceYNoise());
+
+  EXPECT_EQ(defaultNoise, ft.ForceZNoise());
+  ft.SetForceZNoise(noise);
+  EXPECT_EQ(noise, ft.ForceZNoise());
+
+  EXPECT_EQ(defaultNoise, ft.TorqueXNoise());
+  ft.SetTorqueXNoise(noise);
+  EXPECT_EQ(noise, ft.TorqueXNoise());
+
+  EXPECT_EQ(defaultNoise, ft.TorqueYNoise());
+  ft.SetTorqueYNoise(noise);
+  EXPECT_EQ(noise, ft.TorqueYNoise());
+
+  EXPECT_EQ(defaultNoise, ft.TorqueZNoise());
+  ft.SetTorqueZNoise(noise);
+  EXPECT_EQ(noise, ft.TorqueZNoise());
 
   EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD);
   ft.SetFrame(sdf::ForceTorqueFrame::PARENT);

test/integration/joint_dom.cc

@@ -890,9 +890,24 @@ TEST(DOMJoint, Sensors)
   EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
   EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
       forceTorqueSensor->RawPose());
-  auto forceTorqueSensorConfig = forceTorqueSensor->ForceTorqueSensor();
-  ASSERT_NE(nullptr, forceTorqueSensorConfig);
-  EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorConfig->Frame());
+  auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor();
+  ASSERT_NE(nullptr, forceTorqueSensorObj);
+  EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame());
   EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD,
-      forceTorqueSensorConfig->MeasureDirection());
+      forceTorqueSensorObj->MeasureDirection());
+
+  EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean());
+  EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev());
+  EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean());
+  EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev());
+  EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean());
+  EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev());
+
+  EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean());
+  EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev());
+  EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean());
+  EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev());
+  EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean());
+  EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev());
+
 }

test/sdf/joint_sensors.sdf

@@ -11,6 +11,46 @@
         <force_torque>
           <frame>parent</frame>
           <measure_direction>parent_to_child</measure_direction>
+          <force>
+            <x>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.1</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>1</mean>
+                <stddev>1.1</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>2</mean>
+                <stddev>2.1</stddev>
+              </noise>
+            </z>
+          </force>
+          <torque>
+            <x>
+              <noise type="gaussian">
+                <mean>3</mean>
+                <stddev>3.1</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>4</mean>
+                <stddev>4.1</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>5</mean>
+                <stddev>5.1</stddev>
+              </noise>
+            </z>
+          </torque>
         </force_torque>
       </sensor>
     </joint>