From 3a8c212f2001fad3ba2864f716a36b4926e56533 Mon Sep 17 00:00:00 2001 From: Devansh Date: Mon, 16 Aug 2021 06:10:57 -0400 Subject: [PATCH 1/5] Added Force Torque Noise functions + Unit tests Signed-off-by: Devansh --- include/sdf/ForceTorque.hh | 60 +++++++++++++ src/ForceTorque.cc | 173 ++++++++++++++++++++++++++++++++++++- src/ForceTorque_TEST.cc | 32 +++++++ 3 files changed, 264 insertions(+), 1 deletion(-) diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 35889e811..8e21cc261 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -79,6 +79,66 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Get the noise values related to the body-frame force + /// on the X-axis. + /// \return Noise values for the X-axis force. + public: const Noise &ForceXNoise() const; + + /// \brief Set the noise values related to the body-frame force + /// on the X-axis. + /// \param[in] _noise Noise values for the X-axis force. + public: void SetForceXNoise(const Noise &_noise); + + /// \brief Get the noise values related to the body-frame force + /// on the Y-axis. + /// \return Noise values for the Y-axis force. + public: const Noise &ForceYNoise() const; + + /// \brief Set the noise values related to the body-frame force + /// on the Y-axis. + /// \param[in] _noise Noise values for the Y-axis force. + public: void SetForceYNoise(const Noise &_noise); + + /// \brief Get the noise values related to the body-frame force + /// on the Z-axis. + /// \return Noise values for the Z-axis force. + public: const Noise &ForceZNoise() const; + + /// \brief Set the noise values related to the body-frame force + /// on the Z-axis. + /// \param[in] _noise Noise values for the Z-axis force. + public: void SetForceZNoise(const Noise &_noise); + + /// \brief Get the noise values related to the body-frame torque + /// on the X-axis. + /// \return Noise values for the X-axis torque. + public: const Noise &TorqueXNoise() const; + + /// \brief Set the noise values related to the body-frame torque + /// on the X-axis. + /// \param[in] _noise Noise values for the X-axis torque. + public: void SetTorqueXNoise(const Noise &_noise); + + /// \brief Get the noise values related to the body-frame torque + /// on the Y-axis. + /// \return Noise values for the Y-axis torque. + public: const Noise &TorqueYNoise() const; + + /// \brief Set the noise values related to the body-frame torque + /// on the Y-axis. + /// \param[in] _noise Noise values for the Y-axis torque. + public: void SetTorqueYNoise(const Noise &_noise); + + /// \brief Get the noise values related to the body-frame torque + /// on the Z-axis. + /// \return Noise values for the Z-axis torque. + public: const Noise &TorqueZNoise() const; + + /// \brief Set the noise values related to the body-frame torque + /// on the 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; diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index b09560454..72d27dd38 100644 --- a/src/ForceTorque.cc +++ b/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,74 @@ Errors ForceTorque::Load(ElementPtr _sdf) } } + // Load the force noise values. + if(_sdf->HasElement("force")) + { + sdf::ElementPtr force = _sdf->GetElement("force"); + { + if(force->HasElement("x")) + { + if(force->GetElement("x")->HasElement("noise")) + { + this->dataPtr->forceXNoise.Load( + force->GetElement("x")->GetElement("noise")); + } + } + + if(force->HasElement("y")) + { + if(force->GetElement("y")->HasElement("noise")) + { + this->dataPtr->forceXNoise.Load( + force->GetElement("y")->GetElement("noise")); + } + } + + if(force->HasElement("z")) + { + if(force->GetElement("z")->HasElement("noise")) + { + this->dataPtr->forceXNoise.Load( + force->GetElement("z")->GetElement("noise")); + } + } + } + } + + // Load the torque noise values. + if(_sdf->HasElement("torque")) + { + sdf::ElementPtr torque = _sdf->GetElement("torque"); + { + if(torque->HasElement("x")) + { + if(torque->GetElement("x")->HasElement("noise")) + { + this->dataPtr->torqueXNoise.Load( + torque->GetElement("x")->GetElement("noise")); + } + } + + if(torque->HasElement("y")) + { + if(torque->GetElement("y")->HasElement("noise")) + { + this->dataPtr->torqueYNoise.Load( + torque->GetElement("y")->GetElement("noise")); + } + } + + if(torque->HasElement("z")) + { + if(torque->GetElement("z")->HasElement("noise")) + { + this->dataPtr->torqueZNoise.Load( + torque->GetElement("z")->GetElement("noise")); + } + } + } + } + return errors; } @@ -125,7 +217,86 @@ 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; } ////////////////////////////////////////////////// diff --git a/src/ForceTorque_TEST.cc b/src/ForceTorque_TEST.cc index 57bff24f8..6acba8514 100644 --- a/src/ForceTorque_TEST.cc +++ b/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); From eb37f8b02369d1514db6da4616f1e6b6c06b3646 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 16 Sep 2021 15:50:39 -0500 Subject: [PATCH 2/5] Update forcetorque.sdf Signed-off-by: Michael Carroll --- sdf/1.9/forcetorque.sdf | 34 +++++++++++++++++ src/ForceTorque.cc | 81 +++++++++-------------------------------- 2 files changed, 51 insertions(+), 64 deletions(-) diff --git a/sdf/1.9/forcetorque.sdf b/sdf/1.9/forcetorque.sdf index 18e5b8f71..115d34c6f 100644 --- a/sdf/1.9/forcetorque.sdf +++ b/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. + + + These elements are specific to measurement-frame force, + which is expressed in Newtons + + Force along the X axis + + + + Force along the Y axis + + + + Force along the Z axis + + + + + + These elements are specific to measurement-frame torque, + which is expressed in Newton-meters + + Torque about the X axis + + + + Force about the Y axis + + + + Torque about the Z axis + + + diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index 72d27dd38..1042504af 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -130,73 +130,27 @@ Errors ForceTorque::Load(ElementPtr _sdf) } } - // Load the force noise values. - if(_sdf->HasElement("force")) + auto loadAxisNoise = [](sdf::ElementPtr _parent, + const std::string _groupLabel, + const std::string _axisLabel, + sdf::Noise& _noise) { - sdf::ElementPtr force = _sdf->GetElement("force"); + if (_parent->HasElement(_groupLabel) && + _parent->GetElement(_groupLabel)->HasElement(_axisLabel)) { - if(force->HasElement("x")) - { - if(force->GetElement("x")->HasElement("noise")) - { - this->dataPtr->forceXNoise.Load( - force->GetElement("x")->GetElement("noise")); - } - } - - if(force->HasElement("y")) - { - if(force->GetElement("y")->HasElement("noise")) - { - this->dataPtr->forceXNoise.Load( - force->GetElement("y")->GetElement("noise")); - } - } - - if(force->HasElement("z")) - { - if(force->GetElement("z")->HasElement("noise")) - { - this->dataPtr->forceXNoise.Load( - force->GetElement("z")->GetElement("noise")); - } - } + auto axis = _parent->GetElement(_groupLabel)->GetElement(_axisLabel); + _noise.Load(axis->GetElement("noise")); + return true; } - } + return false; + }; - // Load the torque noise values. - if(_sdf->HasElement("torque")) - { - sdf::ElementPtr torque = _sdf->GetElement("torque"); - { - if(torque->HasElement("x")) - { - if(torque->GetElement("x")->HasElement("noise")) - { - this->dataPtr->torqueXNoise.Load( - torque->GetElement("x")->GetElement("noise")); - } - } - - if(torque->HasElement("y")) - { - if(torque->GetElement("y")->HasElement("noise")) - { - this->dataPtr->torqueYNoise.Load( - torque->GetElement("y")->GetElement("noise")); - } - } - - if(torque->HasElement("z")) - { - if(torque->GetElement("z")->HasElement("noise")) - { - this->dataPtr->torqueZNoise.Load( - torque->GetElement("z")->GetElement("noise")); - } - } - } - } + 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; } @@ -218,7 +172,6 @@ bool ForceTorque::operator==(const ForceTorque &_ft) const { return this->dataPtr->frame == _ft.dataPtr->frame && 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 && From dabd816ff1f5fbf824b9f93c13ba520cb05144d1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 21 Sep 2021 10:51:39 -0500 Subject: [PATCH 3/5] Update docs Signed-off-by: Michael Carroll --- include/sdf/ForceTorque.hh | 36 ++++++++++++---------------------- test/sdf/joint_sensors.sdf | 40 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 24 deletions(-) diff --git a/include/sdf/ForceTorque.hh b/include/sdf/ForceTorque.hh index 8e21cc261..172e63ade 100644 --- a/include/sdf/ForceTorque.hh +++ b/include/sdf/ForceTorque.hh @@ -79,63 +79,51 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Get the noise values related to the body-frame force - /// on the X-axis. + /// \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 noise values related to the body-frame force - /// on the X-axis. + /// \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 noise values related to the body-frame force - /// on the Y-axis. + /// \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 noise values related to the body-frame force - /// on the Y-axis. + /// \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 noise values related to the body-frame force - /// on the Z-axis. + /// \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 noise values related to the body-frame force - /// on the Z-axis. + /// \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 noise values related to the body-frame torque - /// on the X-axis. + /// \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 noise values related to the body-frame torque - /// on the X-axis. + /// \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 noise values related to the body-frame torque - /// on the Y-axis. + /// \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 noise values related to the body-frame torque - /// on the Y-axis. + /// \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 noise values related to the body-frame torque - /// on the Z-axis. + /// \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 noise values related to the body-frame torque - /// on the Z-axis. + /// \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); diff --git a/test/sdf/joint_sensors.sdf b/test/sdf/joint_sensors.sdf index 16b63c89b..f4a7a759b 100644 --- a/test/sdf/joint_sensors.sdf +++ b/test/sdf/joint_sensors.sdf @@ -11,6 +11,46 @@ parent parent_to_child + + + + 0 + 0.1 + + + + + 0 + 0.1 + + + + + 0 + 0.1 + + + + + + + 0 + 0.1 + + + + + 0 + 0.1 + + + + + 0 + 0.1 + + + From e4753c259c1107fe7f42401c18ba5d92ecb4df5e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 21 Sep 2021 11:21:40 -0500 Subject: [PATCH 4/5] Expand noise parsing for force-torque sensor Signed-off-by: Michael Carroll --- test/integration/joint_dom.cc | 23 +++++++++++++++++++---- test/sdf/joint_sensors.sdf | 20 ++++++++++---------- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index dc06205af..cda5243d7 100644 --- a/test/integration/joint_dom.cc +++ b/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()); + } diff --git a/test/sdf/joint_sensors.sdf b/test/sdf/joint_sensors.sdf index f4a7a759b..9707c9563 100644 --- a/test/sdf/joint_sensors.sdf +++ b/test/sdf/joint_sensors.sdf @@ -20,34 +20,34 @@ - 0 - 0.1 + 1 + 1.1 - 0 - 0.1 + 2 + 2.1 - 0 - 0.1 + 3 + 3.1 - 0 - 0.1 + 4 + 4.1 - 0 - 0.1 + 5 + 5.1 From 4484116734318728a629d43d4e3c132b2d8aa981 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 21 Sep 2021 13:17:11 -0500 Subject: [PATCH 5/5] Capture nested errors --- src/ForceTorque.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ForceTorque.cc b/src/ForceTorque.cc index 1042504af..01e3f0d63 100644 --- a/src/ForceTorque.cc +++ b/src/ForceTorque.cc @@ -130,7 +130,7 @@ Errors ForceTorque::Load(ElementPtr _sdf) } } - auto loadAxisNoise = [](sdf::ElementPtr _parent, + auto loadAxisNoise = [&errors](sdf::ElementPtr _parent, const std::string _groupLabel, const std::string _axisLabel, sdf::Noise& _noise) @@ -139,7 +139,8 @@ Errors ForceTorque::Load(ElementPtr _sdf) _parent->GetElement(_groupLabel)->HasElement(_axisLabel)) { auto axis = _parent->GetElement(_groupLabel)->GetElement(_axisLabel); - _noise.Load(axis->GetElement("noise")); + sdf::Errors noiseErrors = _noise.Load(axis->GetElement("noise")); + errors.insert(errors.end(), noiseErrors.begin(), noiseErrors.end()); return true; } return false;