Skip to content

Commit

Permalink
JointAxis: update calls to use sdf::Errors output (#1145)
Browse files Browse the repository at this point in the history 
Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org>
  • Loading branch information
Marco A. Gutierrez authored Mar 28, 2023
1 parent 5e97862 commit 64748a3
Show file tree
Hide file tree
Showing 3 changed files with 179 additions and 39 deletions.
10 changes: 10 additions & 0 deletions include/sdf/JointAxis.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
97 changes: 58 additions & 39 deletions src/JointAxis.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,10 +106,11 @@ Errors JointAxis::Load(ElementPtr _sdf)
auto errs = this->SetXyz(_sdf->Get<Vector3d>("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<std::string>("expressed_in");
this->dataPtr->xyzExpressedIn = e->Get<std::string>(
errors, "expressed_in");
}
}
else
Expand All @@ -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<double>("damping",
this->dataPtr->damping).first;
this->dataPtr->friction = dynElement->Get<double>("friction",
this->dataPtr->friction).first;
this->dataPtr->springReference = dynElement->Get<double>("spring_reference",
this->dataPtr->springReference).first;
this->dataPtr->springStiffness = dynElement->Get<double>("spring_stiffness",
this->dataPtr->springStiffness).first;
sdf::ElementPtr dynElement = _sdf->GetElement("dynamics", errors);

this->dataPtr->damping = dynElement->Get<double>(errors,
"damping", this->dataPtr->damping).first;
this->dataPtr->friction = dynElement->Get<double>(errors,
"friction", this->dataPtr->friction).first;
this->dataPtr->springReference = dynElement->Get<double>(errors,
"spring_reference", this->dataPtr->springReference).first;
this->dataPtr->springStiffness = dynElement->Get<double>(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<double>("lower",
this->dataPtr->lower = limitElement->Get<double>(errors, "lower",
this->dataPtr->lower).first;
this->dataPtr->upper = limitElement->Get<double>("upper",
this->dataPtr->upper = limitElement->Get<double>(errors, "upper",
this->dataPtr->upper).first;
this->dataPtr->effort = limitElement->Get<double>("effort",
this->dataPtr->effort = limitElement->Get<double>(errors, "effort",
this->dataPtr->effort).first;
this->dataPtr->maxVelocity = limitElement->Get<double>("velocity",
this->dataPtr->maxVelocity = limitElement->Get<double>(errors, "velocity",
this->dataPtr->maxVelocity).first;
this->dataPtr->stiffness = limitElement->Get<double>("stiffness",
this->dataPtr->stiffness = limitElement->Get<double>(errors, "stiffness",
this->dataPtr->stiffness).first;
this->dataPtr->dissipation = limitElement->Get<double>("dissipation",
this->dataPtr->dissipation).first;
this->dataPtr->dissipation = limitElement->Get<double>(errors,
"dissipation", this->dataPtr->dissipation).first;
}
else
{
Expand Down Expand Up @@ -376,35 +377,53 @@ 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);

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<gz::math::Vector3d>(this->Xyz());
sdf::ElementPtr axisElem = elem->GetElement(axisElemName, _errors);
sdf::ElementPtr xyzElem = axisElem->GetElement("xyz", _errors);
xyzElem->Set<gz::math::Vector3d>(_errors, this->Xyz());
if (!this->XyzExpressedIn().empty())
{
xyzElem->GetAttribute("expressed_in")->Set<std::string>(
this->XyzExpressedIn());
this->XyzExpressedIn(), _errors);
}
sdf::ElementPtr dynElem = axisElem->GetElement("dynamics");
dynElem->GetElement("damping")->Set<double>(this->Damping());
dynElem->GetElement("friction")->Set<double>(this->Friction());
dynElem->GetElement("spring_reference")->Set<double>(
this->SpringReference());
dynElem->GetElement("spring_stiffness")->Set<double>(
this->SpringStiffness());

sdf::ElementPtr limitElem = axisElem->GetElement("limit");
limitElem->GetElement("lower")->Set<double>(this->Lower());
limitElem->GetElement("upper")->Set<double>(this->Upper());
limitElem->GetElement("effort")->Set<double>(this->Effort());
limitElem->GetElement("velocity")->Set<double>(this->MaxVelocity());
limitElem->GetElement("stiffness")->Set<double>(this->Stiffness());
limitElem->GetElement("dissipation")->Set<double>(this->Dissipation());
sdf::ElementPtr dynElem = axisElem->GetElement("dynamics", _errors);
dynElem->GetElement("damping", _errors)->Set<double>(
_errors, this->Damping());
dynElem->GetElement("friction", _errors)->Set<double>(
_errors, this->Friction());
dynElem->GetElement("spring_reference", _errors)->Set<double>(
_errors, this->SpringReference());
dynElem->GetElement("spring_stiffness", _errors)->Set<double>(
_errors, this->SpringStiffness());

sdf::ElementPtr limitElem = axisElem->GetElement("limit", _errors);
limitElem->GetElement("lower", _errors)->Set<double>(
_errors, this->Lower());
limitElem->GetElement("upper", _errors)->Set<double>(
_errors, this->Upper());
limitElem->GetElement("effort", _errors)->Set<double>(
_errors, this->Effort());
limitElem->GetElement("velocity", _errors)->Set<double>(
_errors, this->MaxVelocity());
limitElem->GetElement("stiffness", _errors)->Set<double>(
_errors, this->Stiffness());
limitElem->GetElement("dissipation", _errors)->Set<double>(
_errors, this->Dissipation());
return axisElem;
}
111 changes: 111 additions & 0 deletions src/JointAxis_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <gtest/gtest.h>
#include <limits>
#include "sdf/JointAxis.hh"
#include "test_utils.hh"

/////////////////////////////////////////////////
TEST(DOMJointAxis, Construction)
Expand Down Expand Up @@ -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<gz::math::Vector3d>(
errors, "xyz", xyz).first;
ASSERT_TRUE(errors.empty());
EXPECT_EQ(gz::math::Vector3d::UnitY, axis.Xyz());
std::string expressedIn = elem->GetElement("xyz", errors)->Get<std::string>(
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<double>(errors, "damping", damping).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(0.2, damping);

double friction;
friction = dynElem->Get<double>(errors, "friction", friction).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(1.3, friction);

double springReference;
springReference = dynElem->Get<double>(
errors, "spring_reference", springReference).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(2.4, springReference);

double springStiffness;
springStiffness = dynElem->Get<double>(
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<double>(errors, "lower", lower).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(-10.8, lower);

double upper;
upper = limitElem->Get<double>(errors, "upper", upper).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(123.4, upper);

double effort;
effort = limitElem->Get<double>(errors, "effort", effort).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(3.2, effort);

double maxVel;
maxVel = limitElem->Get<double>(errors, "velocity", maxVel).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(54.2, maxVel);

double stiffness;
stiffness = limitElem->Get<double>(errors, "stiffness", stiffness).first;
ASSERT_TRUE(errors.empty());
EXPECT_DOUBLE_EQ(1e2, stiffness);

double dissipation;
dissipation = limitElem->Get<double>(
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();
}

0 comments on commit 64748a3

Please sign in to comment.