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. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jun 23, 2020 · de80f92 · de80f92
1 parent f1198d5
commit de80f92
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 37 deletions.
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/test/integration/joint_dom.cc b/test/integration/joint_dom.cc
@@ -397,15 +397,15 @@ TEST(DOMJoint, LoadLinkJointSameName17Invalid)
   for (auto e : errors)
     std::cout << e << std::endl;
   EXPECT_FALSE(errors.empty());
-  EXPECT_EQ(2u, errors.size());
+  EXPECT_EQ(7u, errors.size());
   EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::DUPLICATE_NAME);
   EXPECT_NE(std::string::npos,
     errors[0].Message().find(
       "Joint with non-unique name [attachment] detected in model with name "
       "[link_joint_same_name]."));
-  EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::DUPLICATE_NAME);
+  EXPECT_EQ(errors[3].Code(), sdf::ErrorCode::DUPLICATE_NAME);
   EXPECT_NE(std::string::npos,
-    errors[1].Message().find(
+    errors[3].Message().find(
       "Joint with non-unique name [attachment] detected in model with name "
       "[link_joint_same_name]."));
 }