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

The `//joint/child` and `//joint/parent` elements now accept any valid frame name instead of only link names. Closes #204. * Add sdf test models * parser: update joint check function, add tests * FrameSemantics: joint parent/child as frames buildFrameAttachedToGraph: don't create edges from joint to child until all joint and frame vertices are added, in case the child is not a link. buildPoseRelativeToGraph: if //joint/pose/@relative_to is not specified, don't create edges from joint to child until all joint and frame vertices are added to the graph, in case the child is not a link. * Joint: add ResolveChildLink, ResolveParentLink These methods use the FrameAttachedToGraph to resolve the name of the parent and child links of a joint, similar to Frame::ResolveAttachedToBody. Return early from ResolveParentLink if "world" is specified as a joint's parent frame, since "world" is not in a Model's FrameAttachedToGraph. * Check that resolved parent and child frames differ Add test world in which a joint's child frame resolves to the same value as the parent, which is invalid. Check for this case in `ign sdf --check`. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jul 16, 2020 · a460035 · a460035
1 parent 3b4275d
commit a460035
Show file tree

Hide file tree

Showing 15 changed files with 565 additions and 53 deletions.
diff --git a/Migration.md b/Migration.md
@@ -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
@@ -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()
@@ -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

diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh
@@ -35,6 +35,7 @@ namespace sdf
   // Forward declarations.
   class JointAxis;
   class JointPrivate;
+  struct FrameAttachedToGraph;
   struct PoseRelativeToGraph;
 
   /// \enum JointType
@@ -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
@@ -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.

diff --git a/sdf/1.8/joint.sdf b/sdf/1.8/joint.sdf
@@ -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">

diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc
@@ -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);
@@ -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
@@ -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)
   {
@@ -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);
@@ -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
@@ -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());

diff --git a/src/Joint.cc b/src/Joint.cc
@@ -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;
@@ -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;
 };
@@ -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)
@@ -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)
@@ -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
 {

diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc
@@ -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);

diff --git a/src/Model.cc b/src/Model.cc
@@ -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)
@@ -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);