Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

JointAxis: update calls to use sdf::Errors output #1145

Merged
merged 11 commits into from
Mar 28, 2023
Merged

JointAxis: update calls to use sdf::Errors output #1145

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
827b6b7
updating Element calls to use Errors
Sep 15, 2022
29a7e46
JointAxis: adding error vectors to function calls
Sep 16, 2022
284624d
Merge branch 'sdf13' into marcoag/sdf_error_jointAxis
Mar 22, 2023
45e85ed
Changes for new Element error methods
Mar 22, 2023
1cf7648
Fix line length.
Mar 22, 2023
571c153
Switching tabs for spaces
Mar 22, 2023
b8e9e68
add test for JointAxis ToElement method
Mar 23, 2023
0a1097b
fix lines lenght
Mar 23, 2023
67b5ed4
Add misisng errors output to function calls
Mar 24, 2023
96a6865
Add mixing errors to GetElement and typo fix
Mar 28, 2023
402b2b0
Merge branch 'sdf13' into marcoag/sdf_error_jointAxis
Mar 28, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions include/sdf/JointAxis.hh
View file
Open in desktop
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
96 changes: 57 additions & 39 deletions src/JointAxis.cc
View file
Open in desktop
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");

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 @@ -374,37 +375,54 @@ sdf::ElementPtr JointAxis::Element() const
return this->dataPtr->sdf;
}

/////////////////////////////////////////////////
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>(
this->Damping());
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry I didn't notice this last review, I think you might have missed using the new overloaded function with errors here

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good catch, thanks!

dynElem->GetElement("friction", _errors)->Set<double>(
this->Friction());
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same for this line as well

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good catch, thanks!

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;
}