diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index deac2965b..768bcd989 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -229,6 +229,16 @@ namespace sdf /// \return SDF element pointer with updated joint values. public: sdf::ElementPtr ToElement(unsigned int _index = 0u) const; + /// \brief Create and return an SDF element filled with data from this + /// joint axis. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \param[in] _index Index of this joint axis + /// \return SDF element pointer with updated joint values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors, + unsigned int _index = 0u) const; + /// \brief Give the name of the xml parent of this object, to be used /// for resolving poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 735254ea1..39eb5ea50 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -106,10 +106,11 @@ Errors JointAxis::Load(ElementPtr _sdf) 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"); + auto e = _sdf->GetElement("xyz", errors); if (e->HasAttribute("expressed_in")) { - this->dataPtr->xyzExpressedIn = e->Get("expressed_in"); + this->dataPtr->xyzExpressedIn = e->Get( + errors, "expressed_in"); } } else @@ -121,35 +122,35 @@ Errors JointAxis::Load(ElementPtr _sdf) // Load dynamic values, if present if (_sdf->HasElement("dynamics")) { - sdf::ElementPtr dynElement = _sdf->GetElement("dynamics"); - - 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; + sdf::ElementPtr dynElement = _sdf->GetElement("dynamics", errors); + + this->dataPtr->damping = dynElement->Get(errors, + "damping", this->dataPtr->damping).first; + this->dataPtr->friction = dynElement->Get(errors, + "friction", this->dataPtr->friction).first; + this->dataPtr->springReference = dynElement->Get(errors, + "spring_reference", this->dataPtr->springReference).first; + this->dataPtr->springStiffness = dynElement->Get(errors, + "spring_stiffness", this->dataPtr->springStiffness).first; } // Load limit values if (_sdf->HasElement("limit")) { - sdf::ElementPtr limitElement = _sdf->GetElement("limit"); + sdf::ElementPtr limitElement = _sdf->GetElement("limit", errors); - this->dataPtr->lower = limitElement->Get("lower", + this->dataPtr->lower = limitElement->Get(errors, "lower", this->dataPtr->lower).first; - this->dataPtr->upper = limitElement->Get("upper", + this->dataPtr->upper = limitElement->Get(errors, "upper", this->dataPtr->upper).first; - this->dataPtr->effort = limitElement->Get("effort", + this->dataPtr->effort = limitElement->Get(errors, "effort", this->dataPtr->effort).first; - this->dataPtr->maxVelocity = limitElement->Get("velocity", + this->dataPtr->maxVelocity = limitElement->Get(errors, "velocity", this->dataPtr->maxVelocity).first; - this->dataPtr->stiffness = limitElement->Get("stiffness", + this->dataPtr->stiffness = limitElement->Get(errors, "stiffness", this->dataPtr->stiffness).first; - this->dataPtr->dissipation = limitElement->Get("dissipation", - this->dataPtr->dissipation).first; + this->dataPtr->dissipation = limitElement->Get(errors, + "dissipation", this->dataPtr->dissipation).first; } else { @@ -376,6 +377,16 @@ sdf::ElementPtr JointAxis::Element() const ///////////////////////////////////////////////// sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const +{ + sdf::Errors errors; + auto result = this->ToElement(errors, _index); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr JointAxis::ToElement(sdf::Errors &_errors, + unsigned int _index) const { sdf::ElementPtr elem(new sdf::Element); sdf::initFile("joint.sdf", elem); @@ -383,28 +394,36 @@ sdf::ElementPtr JointAxis::ToElement(unsigned int _index) const std::string axisElemName = "axis"; if (_index > 0u) axisElemName += std::to_string(_index + 1); - sdf::ElementPtr axisElem = elem->GetElement(axisElemName); - sdf::ElementPtr xyzElem = axisElem->GetElement("xyz"); - xyzElem->Set(this->Xyz()); + sdf::ElementPtr axisElem = elem->GetElement(axisElemName, _errors); + sdf::ElementPtr xyzElem = axisElem->GetElement("xyz", _errors); + xyzElem->Set(_errors, this->Xyz()); if (!this->XyzExpressedIn().empty()) { xyzElem->GetAttribute("expressed_in")->Set( - this->XyzExpressedIn()); + this->XyzExpressedIn(), _errors); } - sdf::ElementPtr dynElem = axisElem->GetElement("dynamics"); - dynElem->GetElement("damping")->Set(this->Damping()); - dynElem->GetElement("friction")->Set(this->Friction()); - dynElem->GetElement("spring_reference")->Set( - this->SpringReference()); - dynElem->GetElement("spring_stiffness")->Set( - this->SpringStiffness()); - - sdf::ElementPtr limitElem = axisElem->GetElement("limit"); - limitElem->GetElement("lower")->Set(this->Lower()); - limitElem->GetElement("upper")->Set(this->Upper()); - limitElem->GetElement("effort")->Set(this->Effort()); - limitElem->GetElement("velocity")->Set(this->MaxVelocity()); - limitElem->GetElement("stiffness")->Set(this->Stiffness()); - limitElem->GetElement("dissipation")->Set(this->Dissipation()); + sdf::ElementPtr dynElem = axisElem->GetElement("dynamics", _errors); + dynElem->GetElement("damping", _errors)->Set( + _errors, this->Damping()); + dynElem->GetElement("friction", _errors)->Set( + _errors, this->Friction()); + dynElem->GetElement("spring_reference", _errors)->Set( + _errors, this->SpringReference()); + dynElem->GetElement("spring_stiffness", _errors)->Set( + _errors, this->SpringStiffness()); + + sdf::ElementPtr limitElem = axisElem->GetElement("limit", _errors); + limitElem->GetElement("lower", _errors)->Set( + _errors, this->Lower()); + limitElem->GetElement("upper", _errors)->Set( + _errors, this->Upper()); + limitElem->GetElement("effort", _errors)->Set( + _errors, this->Effort()); + limitElem->GetElement("velocity", _errors)->Set( + _errors, this->MaxVelocity()); + limitElem->GetElement("stiffness", _errors)->Set( + _errors, this->Stiffness()); + limitElem->GetElement("dissipation", _errors)->Set( + _errors, this->Dissipation()); return axisElem; } diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 031c989a6..b367ac01c 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -18,6 +18,7 @@ #include #include #include "sdf/JointAxis.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// TEST(DOMJointAxis, Construction) @@ -156,3 +157,113 @@ TEST(DOMJointAxis, ZeroNormVectorReturnsError) ASSERT_FALSE(errors.empty()); EXPECT_EQ(errors[0].Message(), "The norm of the xyz vector cannot be zero"); } + +///////////////////////////////////////////////// +TEST(DOMJointAxis, ToElement) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::JointAxis axis; + sdf::Errors errors; + + errors = axis.SetXyz(gz::math::Vector3d(0, 1, 0)); + ASSERT_TRUE(errors.empty()); + axis.SetXyzExpressedIn("test"); + ASSERT_TRUE(errors.empty()); + + axis.SetDamping(0.2); + axis.SetFriction(1.3); + axis.SetSpringReference(2.4); + axis.SetSpringStiffness(-1.2); + axis.SetLower(-10.8); + axis.SetUpper(123.4); + axis.SetEffort(3.2); + axis.SetMaxVelocity(54.2); + axis.SetStiffness(1e2); + axis.SetDissipation(1.5); + + sdf::ElementPtr elem = axis.ToElement(errors); + ASSERT_TRUE(errors.empty()); + + sdf::ElementPtr xyzElem = elem->GetElement("xyz", errors); + ASSERT_TRUE(errors.empty()); + gz::math::Vector3d xyz = elem->Get( + errors, "xyz", xyz).first; + ASSERT_TRUE(errors.empty()); + EXPECT_EQ(gz::math::Vector3d::UnitY, axis.Xyz()); + std::string expressedIn = elem->GetElement("xyz", errors)->Get( + errors, "expressed_in"); + ASSERT_TRUE(errors.empty()); + EXPECT_EQ("test", expressedIn); + + sdf::ElementPtr dynElem = elem->GetElement("dynamics", errors); + ASSERT_TRUE(errors.empty()); + + double damping; + damping = dynElem->Get(errors, "damping", damping).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(0.2, damping); + + double friction; + friction = dynElem->Get(errors, "friction", friction).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(1.3, friction); + + double springReference; + springReference = dynElem->Get( + errors, "spring_reference", springReference).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(2.4, springReference); + + double springStiffness; + springStiffness = dynElem->Get( + errors, "spring_stiffness", springStiffness).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(-1.2, springStiffness); + + sdf::ElementPtr limitElem = elem->GetElement("limit", errors); + double lower; + lower = limitElem->Get(errors, "lower", lower).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(-10.8, lower); + + double upper; + upper = limitElem->Get(errors, "upper", upper).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(123.4, upper); + + double effort; + effort = limitElem->Get(errors, "effort", effort).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(3.2, effort); + + double maxVel; + maxVel = limitElem->Get(errors, "velocity", maxVel).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(54.2, maxVel); + + double stiffness; + stiffness = limitElem->Get(errors, "stiffness", stiffness).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(1e2, stiffness); + + double dissipation; + dissipation = limitElem->Get( + errors, "dissipation", dissipation).first; + ASSERT_TRUE(errors.empty()); + EXPECT_DOUBLE_EQ(1.5, dissipation); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +}