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.

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

sdf 1.8: support specifying frames as joint child/parent #304

Merged
merged 12 commits into from
Jul 16, 2020
15 changes: 14 additions & 1 deletion Migration.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,14 @@ forward programmatically.
This document aims to contain similar information to those files
but with improved human-readability..

## SDFormat 10.x to 11.0

### Additions

1. **sdf/Joint.hh**
+ Errors ResolveChildLink(std::string&) const
+ Errors ResolveParentLink(std::string&) const

## SDFormat 9.x to 10.0

### Modifications
Expand All @@ -27,7 +35,7 @@ but with improved human-readability..
1. + Removed the `parser_urdf.hh` header file and its `URDF2SDF` class
+ [Pull request 276](https://github.com/osrf/sdformat/pull/276)

1. + Removed the deprecated `Pose()`, `SetPose(), and `*PoseFrame()` API's in all DOM classes:
1. + Removed the deprecated `Pose()`, `SetPose()`, and `*PoseFrame()` API's in all DOM classes:
+ const ignition::math::Pose3d &Pose()
+ void SetPose(const ignition::math::Pose3d &)
+ const std::string &PoseFrame()
Expand Down Expand Up @@ -221,6 +229,11 @@ but with improved human-readability..

## SDF protocol 1.7 to 1.8

### Modifications

1. **joint.sdf** `child` and `parent` elements accept frame names instead of only link names
* [Issue 204](https://github.com/osrf/sdformat/issues/204)

### Deprecations

1. **joint.sdf** `initial_position` element in `<joint><axis>` and `<joint><axis2>` is deprecated
Expand Down
20 changes: 20 additions & 0 deletions include/sdf/Joint.hh
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ namespace sdf
// Forward declarations.
class JointAxis;
class JointPrivate;
struct FrameAttachedToGraph;
struct PoseRelativeToGraph;

/// \enum JointType
Expand Down Expand Up @@ -147,6 +148,18 @@ namespace sdf
/// \param[in] _name Name of the child link.
public: void SetChildLinkName(const std::string &_name);

/// \brief Resolve the name of the child link from the
/// FrameAttachedToGraph.
/// \param[out] _body Name of child link of this joint.
/// \return Errors.
public: Errors ResolveChildLink(std::string &_link) const;

/// \brief Resolve the name of the parent link from the
/// FrameAttachedToGraph. It will return the name of a link or "world".
/// \param[out] _body Name of parent link of this joint.
/// \return Errors.
public: Errors ResolveParentLink(std::string &_link) const;

/// \brief Get a joint axis.
/// \param[in] _index This value specifies which axis to get. A value of
/// zero corresponds to the first axis, which is the <axis> SDF
Expand Down Expand Up @@ -208,6 +221,13 @@ namespace sdf
/// \return SemanticPose object for this link.
public: sdf::SemanticPose SemanticPose() const;

/// \brief Give a weak pointer to the FrameAttachedToGraph to be used
/// for resolving parent and child link names. This is private and is
/// intended to be called by Model::Load.
/// \param[in] _graph Weak pointer to FrameAttachedToGraph.
private: void SetFrameAttachedToGraph(
std::weak_ptr<const FrameAttachedToGraph> _graph);

/// \brief Give a weak pointer to the PoseRelativeToGraph to be used
/// for resolving poses. This is private and is intended to be called by
/// Model::Load.
Expand Down
4 changes: 2 additions & 2 deletions sdf/1.8/joint.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,11 @@
</attribute>

<element name="parent" type="string" default="__default__" required="1">
<description>Name of the parent link or "world".</description>
<description>Name of the parent frame or "world".</description>
</element> <!-- End Parent -->

<element name="child" type="string" default="__default__" required="1">
<description>Name of the child link. The value "world" may not be specified.</description>
<description>Name of the child frame. The value "world" may not be specified.</description>
</element> <!-- End Child -->

<element name="gearbox_ratio" type="double" default="1.0" required="0">
Expand Down
58 changes: 24 additions & 34 deletions src/FrameSemantics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,7 @@ Errors buildFrameAttachedToGraph(
}
}

// add joint vertices and edges to child link
// add joint vertices
for (uint64_t j = 0; j < _model->JointCount(); ++j)
{
auto joint = _model->JointByIndex(j);
Expand All @@ -254,18 +254,6 @@ Errors buildFrameAttachedToGraph(
auto jointId =
_out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id();
_out.map[joint->Name()] = jointId;

auto childLink = _model->LinkByName(joint->ChildLinkName());
if (nullptr == childLink)
{
errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID,
"Child link with name[" + joint->ChildLinkName() +
"] specified by joint with name[" + joint->Name() +
"] not found in model with name[" + _model->Name() + "]."});
continue;
}
auto childLinkId = _out.map.at(childLink->Name());
_out.graph.AddEdge({jointId, childLinkId}, true);
}

// add frame vertices
Expand All @@ -285,6 +273,24 @@ Errors buildFrameAttachedToGraph(
_out.map[frame->Name()] = frameId;
}

// add edges from joint to child frames
for (uint64_t j = 0; j < _model->JointCount(); ++j)
{
auto joint = _model->JointByIndex(j);
auto jointId = _out.map.at(joint->Name());
auto childFrameName = joint->ChildLinkName();
if (_out.map.count(childFrameName) != 1)
{
errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID,
"Child frame with name[" + childFrameName +
"] specified by joint with name[" + joint->Name() +
"] not found in model with name[" + _model->Name() + "]."});
continue;
}
auto childFrameId = _out.map.at(childFrameName);
_out.graph.AddEdge({jointId, childFrameId}, true);
}

// add frame edges
for (uint64_t f = 0; f < _model->FrameCount(); ++f)
{
Expand Down Expand Up @@ -480,7 +486,7 @@ Errors buildPoseRelativeToGraph(
}
}

// add joint vertices and default edge if relative_to is empty
// add joint vertices
for (uint64_t j = 0; j < _model->JointCount(); ++j)
{
auto joint = _model->JointByIndex(j);
Expand All @@ -495,22 +501,6 @@ Errors buildPoseRelativeToGraph(
auto jointId =
_out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id();
_out.map[joint->Name()] = jointId;

if (joint->PoseRelativeTo().empty())
{
// relative_to is empty, so add edge from joint to child link
auto childLink = _model->LinkByName(joint->ChildLinkName());
if (nullptr == childLink)
{
errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID,
"Child link with name[" + joint->ChildLinkName() +
"] specified by joint with name[" + joint->Name() +
"] not found in model with name[" + _model->Name() + "]."});
continue;
}
auto childLinkId = _out.map.at(childLink->Name());
_out.graph.AddEdge({childLinkId, jointId}, joint->RawPose());
}
}

// add frame vertices and default edge if both
Expand Down Expand Up @@ -579,11 +569,11 @@ Errors buildPoseRelativeToGraph(
{
auto joint = _model->JointByIndex(j);

// check if we've already added a default edge
const std::string relativeTo = joint->PoseRelativeTo();
if (joint->PoseRelativeTo().empty())
std::string relativeTo = joint->PoseRelativeTo();
if (relativeTo.empty())
{
continue;
// since nothing else was specified, use the joint's child frame
relativeTo = joint->ChildLinkName();
}

auto jointId = _out.map.at(joint->Name());
Expand Down
64 changes: 64 additions & 0 deletions src/Joint.cc
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include "sdf/Joint.hh"
#include "sdf/JointAxis.hh"
#include "sdf/Types.hh"
#include "FrameSemantics.hh"
#include "Utils.hh"

using namespace sdf;
Expand Down Expand Up @@ -73,6 +74,9 @@ class sdf::JointPrivate
/// \brief The SDF element pointer used during load.
public: sdf::ElementPtr sdf;

/// \brief Weak pointer to model's Frame Attached-To Graph.
public: std::weak_ptr<const sdf::FrameAttachedToGraph> frameAttachedToGraph;

/// \brief Weak pointer to model's Pose Relative-To Graph.
public: std::weak_ptr<const sdf::PoseRelativeToGraph> poseRelativeToGraph;
};
Expand All @@ -87,6 +91,7 @@ JointPrivate::JointPrivate(const JointPrivate &_jointPrivate)
poseRelativeTo(_jointPrivate.poseRelativeTo),
threadPitch(_jointPrivate.threadPitch),
sdf(_jointPrivate.sdf),
frameAttachedToGraph(_jointPrivate.frameAttachedToGraph),
poseRelativeToGraph(_jointPrivate.poseRelativeToGraph)
{
for (std::size_t i = 0; i < _jointPrivate.axis.size(); ++i)
Expand Down Expand Up @@ -354,6 +359,13 @@ void Joint::SetPoseRelativeTo(const std::string &_frame)
this->dataPtr->poseRelativeTo = _frame;
}

/////////////////////////////////////////////////
void Joint::SetFrameAttachedToGraph(
std::weak_ptr<const FrameAttachedToGraph> _graph)
{
this->dataPtr->frameAttachedToGraph = _graph;
}

/////////////////////////////////////////////////
void Joint::SetPoseRelativeToGraph(
std::weak_ptr<const PoseRelativeToGraph> _graph)
Expand All @@ -370,6 +382,58 @@ void Joint::SetPoseRelativeToGraph(
}
}

/////////////////////////////////////////////////
Errors Joint::ResolveChildLink(std::string &_link) const
{
Errors errors;

auto graph = this->dataPtr->frameAttachedToGraph.lock();
if (!graph)
{
errors.push_back({ErrorCode::ELEMENT_INVALID,
"Frame has invalid pointer to FrameAttachedToGraph."});
return errors;
}

std::string link;
errors = resolveFrameAttachedToBody(link, *graph, this->ChildLinkName());
if (errors.empty())
{
_link = link;
}
return errors;
}

/////////////////////////////////////////////////
Errors Joint::ResolveParentLink(std::string &_link) const
{
Errors errors;

// special case for world, return without resolving since it's not in a
// model's FrameAttachedToGraph
if ("world" == this->ParentLinkName())
{
_link = "world";
return errors;
}

auto graph = this->dataPtr->frameAttachedToGraph.lock();
if (!graph)
{
errors.push_back({ErrorCode::ELEMENT_INVALID,
"Frame has invalid pointer to FrameAttachedToGraph."});
return errors;
}

std::string link;
errors = resolveFrameAttachedToBody(link, *graph, this->ParentLinkName());
if (errors.empty())
{
_link = link;
}
return errors;
}

/////////////////////////////////////////////////
sdf::SemanticPose Joint::SemanticPose() const
{
Expand Down
6 changes: 6 additions & 0 deletions src/Joint_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,12 @@ TEST(DOMJoint, Construction)
joint.SetChildLinkName("child");
EXPECT_EQ("child", joint.ChildLinkName());

std::string body;
EXPECT_FALSE(joint.ResolveChildLink(body).empty());
EXPECT_TRUE(body.empty());
EXPECT_FALSE(joint.ResolveParentLink(body).empty());
EXPECT_TRUE(body.empty());

joint.SetType(sdf::JointType::BALL);
EXPECT_EQ(sdf::JointType::BALL, joint.Type());
joint.SetType(sdf::JointType::CONTINUOUS);
Expand Down
5 changes: 5 additions & 0 deletions src/Model.cc
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ Model::Model(const Model &_model)
}
for (auto &joint : this->dataPtr->joints)
{
joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
}
for (auto &frame : this->dataPtr->frames)
Expand Down Expand Up @@ -328,6 +329,10 @@ Errors Model::Load(ElementPtr _sdf)
validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph);
errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(),
validateFrameAttachedGraphErrors.end());
for (auto &joint : this->dataPtr->joints)
{
joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
}
for (auto &frame : this->dataPtr->frames)
{
frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
Expand Down
Loading