From fa7fa2545a6eca903ba825db875e52d54a5ac70f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 11 Aug 2022 21:46:09 -0700 Subject: [PATCH 1/3] 1.10/joint.sdf: better default limit values MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use `±inf` instead of `±1e16` for position limits * Use `inf` instead of `-1` for effort, velocity limits Signed-off-by: Steve Peters --- Migration.md | 6 ++++++ python/test/pyJointAxis_TEST.py | 4 ++-- sdf/1.10/joint.sdf | 16 ++++++++-------- src/JointAxis.cc | 18 ++++++++++-------- src/JointAxis_TEST.cc | 5 +++-- test/integration/joint_axis_dom.cc | 4 ++-- 6 files changed, 31 insertions(+), 22 deletions(-) diff --git a/Migration.md b/Migration.md index 7a98ba21c..0d297b7ca 100644 --- a/Migration.md +++ b/Migration.md @@ -538,6 +538,12 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ### Modifications +1. **joint.sdf**: axis limits default values have changed + + `//limit/lower`: `-inf` (formerly `-1e16`) + + `//limit/upper`: `inf` (formerly `1e16`) + + `//limit/velocity`: `inf` (formerly `-1`) + + `//limit/effort`: `inf` (formerly `-1`) + 1. **plugin.sdf**: name attribute is now optional with empty default value. ## SDFormat specification 1.8 to 1.9 diff --git a/python/test/pyJointAxis_TEST.py b/python/test/pyJointAxis_TEST.py index d9ac24aab..b5f68f34f 100644 --- a/python/test/pyJointAxis_TEST.py +++ b/python/test/pyJointAxis_TEST.py @@ -29,8 +29,8 @@ def test_default_construction(self): self.assertAlmostEqual(0.0, axis.friction()) self.assertAlmostEqual(0.0, axis.spring_reference()) self.assertAlmostEqual(0.0, axis.spring_stiffness()) - self.assertAlmostEqual(-1e16, axis.lower()) - self.assertAlmostEqual(1e16, axis.upper()) + self.assertAlmostEqual(-math.inf, axis.lower()) + self.assertAlmostEqual(math.inf, axis.upper()) self.assertAlmostEqual(math.inf, axis.effort()) self.assertAlmostEqual(math.inf, axis.max_velocity()) self.assertAlmostEqual(1e8, axis.stiffness()) diff --git a/sdf/1.10/joint.sdf b/sdf/1.10/joint.sdf index d472feefa..04d5f52e1 100644 --- a/sdf/1.10/joint.sdf +++ b/sdf/1.10/joint.sdf @@ -74,16 +74,16 @@ specifies the limits of this joint - + Specifies the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + Specifies the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + A value for enforcing the maximum joint effort applied. Limit is not enforced if value is negative. - + A value for enforcing the maximum joint velocity. @@ -132,16 +132,16 @@ - + An attribute specifying the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. - + An attribute for enforcing the maximum joint effort applied by Joint::SetForce. Limit is not enforced if value is negative. - + (not implemented) An attribute for enforcing the maximum joint velocity. diff --git a/src/JointAxis.cc b/src/JointAxis.cc index de3ef479d..5f21686d5 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -60,18 +61,18 @@ class sdf::JointAxis::Implementation /// \brief Specifies the lower joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double lower = -1e16; + public: double lower = -std::numeric_limits::infinity(); /// \brief Specifies the upper joint limit (radians for revolute joints, /// meters for prismatic joints). Omit if joint is continuous. - public: double upper = 1e16; + public: double upper = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint effort applied. /// Limit is not enforced if value is negative. - public: double effort = -1; + public: double effort = std::numeric_limits::infinity(); /// \brief A value for enforcing the maximum joint velocity. - public: double maxVelocity = -1; + public: double maxVelocity = std::numeric_limits::infinity(); /// \brief Joint stop stiffness. public: double stiffness = 1e8; @@ -142,11 +143,12 @@ Errors JointAxis::Load(ElementPtr _sdf) { sdf::ElementPtr limitElement = _sdf->GetElement("limit"); - this->dataPtr->lower = limitElement->Get("lower", -1e16).first; - this->dataPtr->upper = limitElement->Get("upper", 1e16).first; - this->dataPtr->effort = limitElement->Get("effort", -1).first; + const double kInf = std::numeric_limits::infinity(); + this->dataPtr->lower = limitElement->Get("lower", -kInf).first; + this->dataPtr->upper = limitElement->Get("upper", kInf).first; + this->dataPtr->effort = limitElement->Get("effort", kInf).first; this->dataPtr->maxVelocity = limitElement->Get( - "velocity", -1).first; + "velocity", kInf).first; this->dataPtr->stiffness = limitElement->Get( "stiffness", 1e8).first; this->dataPtr->dissipation = limitElement->Get( diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 546dae334..031c989a6 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/JointAxis.hh" ///////////////////////////////////////////////// @@ -29,8 +30,8 @@ TEST(DOMJointAxis, Construction) EXPECT_DOUBLE_EQ(0.0, axis.Friction()); EXPECT_DOUBLE_EQ(0.0, axis.SpringReference()); EXPECT_DOUBLE_EQ(0.0, axis.SpringStiffness()); - EXPECT_DOUBLE_EQ(-1e16, axis.Lower()); - EXPECT_DOUBLE_EQ(1e16, axis.Upper()); + EXPECT_DOUBLE_EQ(-std::numeric_limits::infinity(), axis.Lower()); + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Upper()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.Effort()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), axis.MaxVelocity()); EXPECT_DOUBLE_EQ(1e8, axis.Stiffness()); diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc index 3813dd4dd..b1c102e28 100644 --- a/test/integration/joint_axis_dom.cc +++ b/test/integration/joint_axis_dom.cc @@ -244,8 +244,8 @@ TEST(DOMJointAxis, InfiniteLimits) ASSERT_NE(nullptr, joint); auto axis = joint->Axis(0); ASSERT_NE(nullptr, axis); - EXPECT_DOUBLE_EQ(-1e16, axis->Lower()); - EXPECT_DOUBLE_EQ(1e16, axis->Upper()); + EXPECT_DOUBLE_EQ(-kInf, axis->Lower()); + EXPECT_DOUBLE_EQ(kInf, axis->Upper()); EXPECT_DOUBLE_EQ(kInf, axis->Effort()); EXPECT_DOUBLE_EQ(kInf, axis->MaxVelocity()); } From c7feca99e73fdc684a9bc4705510274294d09ef5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 17 Aug 2022 02:26:51 -0700 Subject: [PATCH 2/3] Deduplicate default values Signed-off-by: Steve Peters --- src/JointAxis.cc | 39 ++++++++++++++++++++++----------------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 5f21686d5..b508ca818 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -107,7 +107,8 @@ Errors JointAxis::Load(ElementPtr _sdf) if (_sdf->HasElement("xyz")) { using gz::math::Vector3d; - auto errs = this->SetXyz(_sdf->Get("xyz", Vector3d::UnitZ).first); + auto errs = this->SetXyz(_sdf->Get("xyz", + this->dataPtr->xyz).first); std::copy(errs.begin(), errs.end(), std::back_inserter(errors)); auto e = _sdf->GetElement("xyz"); if (e->HasAttribute("expressed_in")) @@ -130,12 +131,14 @@ Errors JointAxis::Load(ElementPtr _sdf) { sdf::ElementPtr dynElement = _sdf->GetElement("dynamics"); - this->dataPtr->damping = dynElement->Get("damping", 0.0).first; - this->dataPtr->friction = dynElement->Get("friction", 0.0).first; - this->dataPtr->springReference = - dynElement->Get("spring_reference", 0.0).first; - this->dataPtr->springStiffness = - dynElement->Get("spring_stiffness", 0.0).first; + this->dataPtr->damping = dynElement->Get("damping", + this->dataPtr->damping).first; + this->dataPtr->friction = dynElement->Get("friction", + this->dataPtr->friction).first; + this->dataPtr->springReference = dynElement->Get("spring_reference", + this->dataPtr->springReference).first; + this->dataPtr->springStiffness = dynElement->Get("spring_stiffness", + this->dataPtr->springStiffness).first; } // Load limit values @@ -143,16 +146,18 @@ Errors JointAxis::Load(ElementPtr _sdf) { sdf::ElementPtr limitElement = _sdf->GetElement("limit"); - const double kInf = std::numeric_limits::infinity(); - this->dataPtr->lower = limitElement->Get("lower", -kInf).first; - this->dataPtr->upper = limitElement->Get("upper", kInf).first; - this->dataPtr->effort = limitElement->Get("effort", kInf).first; - this->dataPtr->maxVelocity = limitElement->Get( - "velocity", kInf).first; - this->dataPtr->stiffness = limitElement->Get( - "stiffness", 1e8).first; - this->dataPtr->dissipation = limitElement->Get( - "dissipation", 1.0).first; + this->dataPtr->lower = limitElement->Get("lower", + this->dataPtr->lower).first; + this->dataPtr->upper = limitElement->Get("upper", + this->dataPtr->upper).first; + this->dataPtr->effort = limitElement->Get("effort", + this->dataPtr->effort).first; + this->dataPtr->maxVelocity = limitElement->Get("velocity", + this->dataPtr->maxVelocity).first; + this->dataPtr->stiffness = limitElement->Get("stiffness", + this->dataPtr->stiffness).first; + this->dataPtr->dissipation = limitElement->Get("dissipation", + this->dataPtr->dissipation).first; } else { From 95b3963d430eda20c301135dc53f3f2022d34710 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 17 Aug 2022 02:27:51 -0700 Subject: [PATCH 3/3] JointAxis: remove unused variable The use_parent_model_frame element has already been deprecated and removed, so we can remove the internal variable. Signed-off-by: Steve Peters --- src/JointAxis.cc | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/JointAxis.cc b/src/JointAxis.cc index b508ca818..735254ea1 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -42,10 +42,6 @@ class sdf::JointAxis::Implementation /// \brief Frame in which xyz is expressed in. public: std::string xyzExpressedIn = ""; - /// \brief Flag to interpret the axis xyz element in the parent model - /// frame instead of joint frame. - public: bool useParentModelFrame = false; - /// \brief The physical velocity dependent viscous damping coefficient /// of the joint. public: double damping = 0.0; @@ -122,10 +118,6 @@ Errors JointAxis::Load(ElementPtr _sdf) "The xyz element in joint axis is required"}); } - // Get whether to use the parent model frame. - this->dataPtr->useParentModelFrame = _sdf->Get( - "use_parent_model_frame", false).first; - // Load dynamic values, if present if (_sdf->HasElement("dynamics")) { @@ -399,11 +391,6 @@ sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const xyzElem->GetAttribute("expressed_in")->Set( this->XyzExpressedIn()); } - else if (this->dataPtr->useParentModelFrame) - { - xyzElem->GetAttribute("expressed_in")->Set( - "__model__"); - } sdf::ElementPtr dynElem = axisElem->GetElement("dynamics"); dynElem->GetElement("damping")->Set(this->Damping()); dynElem->GetElement("friction")->Set(this->Friction());