diff --git a/Migration.md b/Migration.md index 0029c6f84..a28387e6a 100644 --- 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 `` and `` is deprecated diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 395977926..407d863c3 100644 --- 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 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 _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 index 42a8cfe99..f58fa75fd 100644 --- a/sdf/1.8/joint.sdf +++ b/sdf/1.8/joint.sdf @@ -21,11 +21,11 @@ - Name of the parent link or "world". + Name of the parent frame or "world". - Name of the child link. The value "world" may not be specified. + Name of the child frame. The value "world" may not be specified. diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 6f606e3a4..d93af7f21 100644 --- 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 index 51018fff3..4a467cbfc 100644 --- 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 frameAttachedToGraph; + /// \brief Weak pointer to model's Pose Relative-To Graph. public: std::weak_ptr 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 _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; +} + ///////////////////////////////////////////////// void Joint::SetPoseRelativeToGraph( std::weak_ptr _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 index 1f1fc6100..79709bb1c 100644 --- 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 index f60593a0e..a5e94c3bf 100644 --- 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); diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index b98c6e752..4dbe7442b 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -197,7 +197,7 @@ TEST(check, SDF) // Check joint_invalid_child.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Child link with name[invalid] specified by " + EXPECT_NE(output.find("Error: Child frame with name[invalid] specified by " "joint with name[joint] not found in model with " "name[joint_invalid_child]."), std::string::npos) << output; @@ -210,12 +210,37 @@ TEST(check, SDF) // Check joint_invalid_parent.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: parent link with name[invalid] specified by " + EXPECT_NE(output.find("Error: parent frame with name[invalid] specified by " "joint with name[joint] not found in model with " "name[joint_invalid_parent]."), std::string::npos) << output; } + // Check an SDF file with a joint that names itself as the child frame. + { + std::string path = pathBase +"/joint_invalid_self_child.sdf"; + + // Check joint_invalid_self_child.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, " + "already visited vertex [self]."), + std::string::npos) << output; + } + + // Check an SDF file with a joint that names itself as the parent frame. + { + std::string path = pathBase +"/joint_invalid_self_parent.sdf"; + + // Check joint_invalid_self_parent.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("Error: joint with name[self] in model with " + "name[joint_invalid_self_parent] must not specify " + "its own name as the parent frame."), + std::string::npos) << output; + } + // Check an SDF file with a joint with identical parent and child. { std::string path = pathBase +"/joint_invalid_parent_same_as_child.sdf"; @@ -229,6 +254,21 @@ TEST(check, SDF) std::string::npos) << output; } + // Check an SDF file with a joint with parent parent frame that resolves + // to the same value as the child. + { + std::string path = + pathBase + "/joint_invalid_resolved_parent_same_as_child.sdf"; + + // Check joint_invalid_resolved_parent_same_as_child.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE(output.find("specified parent frame [J1] and child frame [L2] " + "that both resolve to [L2], but they should resolve " + "to different values."), + std::string::npos) << output; + } + // Check an SDF file with the world specified as a child link. { std::string path = pathBase +"/joint_child_world.sdf"; @@ -252,6 +292,28 @@ TEST(check, SDF) EXPECT_EQ("Valid.\n", output) << output; } + // Check an SDF file with a frame specified as the joint child. + // This is a valid file. + { + std::string path = pathBase +"/joint_child_frame.sdf"; + + // Check joint_child_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + + // Check an SDF file with a frame specified as the joint parent. + // This is a valid file. + { + std::string path = pathBase +"/joint_parent_frame.sdf"; + + // Check joint_parent_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } + // Check an SDF file with the second link specified as the canonical link. // This is a valid file. { diff --git a/src/parser.cc b/src/parser.cc index 3e8279d91..a266c25fd 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1839,9 +1839,11 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) auto joint = _model->JointByIndex(j); const std::string &parentName = joint->ParentLinkName(); - if (parentName != "world" && !_model->LinkNameExists(parentName)) + if (parentName != "world" && !_model->LinkNameExists(parentName) && + !_model->JointNameExists(parentName) && + !_model->FrameNameExists(parentName)) { - std::cerr << "Error: parent link with name[" << parentName + std::cerr << "Error: parent frame with name[" << parentName << "] specified by joint with name[" << joint->Name() << "] not found in model with name[" << _model->Name() << "]." @@ -1850,9 +1852,21 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) } const std::string &childName = joint->ChildLinkName(); - if (childName != "world" && !_model->LinkNameExists(childName)) + if (childName == "world") { - std::cerr << "Error: child link with name[" << childName + std::cerr << "Error: invalid child name[world" + << "] specified by joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "]." + << std::endl; + modelResult = false; + } + + if (!_model->LinkNameExists(childName) && + !_model->JointNameExists(childName) && + !_model->FrameNameExists(childName)) + { + std::cerr << "Error: child frame with name[" << childName << "] specified by joint with name[" << joint->Name() << "] not found in model with name[" << _model->Name() << "]." @@ -1860,13 +1874,57 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) modelResult = false; } - if (childName == parentName) + if (childName == joint->Name()) + { + std::cerr << "Error: joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "] must not specify its own name as the child frame." + << std::endl; + modelResult = false; + } + + if (parentName == joint->Name()) + { + std::cerr << "Error: joint with name[" << joint->Name() + << "] in model with name[" << _model->Name() + << "] must not specify its own name as the parent frame." + << std::endl; + modelResult = false; + } + + // Check that parent and child frames resolve to different links + std::string resolvedChildName; + std::string resolvedParentName; + auto errors = joint->ResolveChildLink(resolvedChildName); + if (!errors.empty()) + { + std::cerr << "Error when attempting to resolve child link name:" + << std::endl; + for (auto error : errors) + { + std::cerr << error.Message() << std::endl; + } + modelResult = false; + } + errors = joint->ResolveParentLink(resolvedParentName); + if (!errors.empty()) + { + std::cerr << "Error when attempting to resolve parent link name:" + << std::endl; + for (auto error : errors) + { + std::cerr << error.Message() << std::endl; + } + modelResult = false; + } + if (resolvedChildName == resolvedParentName) { std::cerr << "Error: joint with name[" << joint->Name() << "] in model with name[" << _model->Name() - << "] must specify different link names for " - << "parent and child, while [" << childName - << "] was specified for both." + << "] specified parent frame [" << parentName + << "] and child frame [" << childName + << "] that both resolve to [" << resolvedChildName + << "], but they should resolve to different values." << std::endl; modelResult = false; } diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index a7d7b0cd3..c1c881c9c 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -20,6 +20,7 @@ #include "sdf/Element.hh" #include "sdf/Filesystem.hh" +#include "sdf/Frame.hh" #include "sdf/Joint.hh" #include "sdf/JointAxis.hh" #include "sdf/Link.hh" @@ -192,9 +193,16 @@ TEST(DOMJoint, LoadJointParentWorld) ASSERT_TRUE(model->JointNameExists("joint")); EXPECT_EQ("link", model->JointByName("joint")->ChildLinkName()); EXPECT_EQ("world", model->JointByName("joint")->ParentLinkName()); - EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("world", resolvedLinkName); EXPECT_EQ(Pose(0, 0, 3, 0, 0, 0), model->JointByName("joint")->RawPose()); + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); EXPECT_EQ(0u, model->FrameCount()); EXPECT_EQ(nullptr, model->FrameByIndex(0)); @@ -221,10 +229,196 @@ TEST(DOMJoint, LoadInvalidJointChildWorld) EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[1].Message().find( - "Child link with name[world] specified by joint with name[joint] " + "Child frame with name[world] specified by joint with name[joint] " "not found in model with name[joint_child_world]")); } +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointParentFrame) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_parent_frame.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("joint_parent_frame", model->Name()); + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_NE(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(nullptr, model->LinkByIndex(2)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("parent_link")); + ASSERT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkByName("parent_link")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->LinkByName("child_link")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + model->LinkByName("parent_link")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + model->LinkByName("child_link")->RawPose()); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(1u, model->JointCount()); + EXPECT_NE(nullptr, model->JointByIndex(0)); + EXPECT_EQ(nullptr, model->JointByIndex(1)); + ASSERT_TRUE(model->JointNameExists("joint")); + EXPECT_EQ("child_link", model->JointByName("joint")->ChildLinkName()); + EXPECT_EQ("parent_frame", model->JointByName("joint")->ParentLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_link", resolvedLinkName); + + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), model->JointByName("joint")->RawPose()); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + ASSERT_TRUE(model->FrameNameExists("parent_frame")); + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), + model->FrameByName("parent_frame")->RawPose()); + + // Test ResolveFrame to get each link, joint and frame pose in model frame. + Pose pose; + EXPECT_TRUE( + model->LinkByName("parent_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + model->LinkByName("child_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->FrameByName("parent_frame")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); + + // joint frame relative to parent and child links + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "child_link").empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "parent_link").empty()); + EXPECT_EQ(Pose(0, 1, 9, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointChildFrame) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_child_frame.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("joint_child_frame", model->Name()); + EXPECT_EQ(2u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_NE(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(nullptr, model->LinkByIndex(2)); + EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ("", model->PoseRelativeTo()); + + ASSERT_TRUE(model->LinkNameExists("parent_link")); + ASSERT_TRUE(model->LinkNameExists("child_link")); + EXPECT_TRUE(model->LinkByName("parent_link")->PoseRelativeTo().empty()); + EXPECT_TRUE(model->LinkByName("child_link")->PoseRelativeTo().empty()); + + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), + model->LinkByName("parent_link")->RawPose()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), + model->LinkByName("child_link")->RawPose()); + + EXPECT_TRUE(model->CanonicalLinkName().empty()); + + EXPECT_EQ(1u, model->JointCount()); + EXPECT_NE(nullptr, model->JointByIndex(0)); + EXPECT_EQ(nullptr, model->JointByIndex(1)); + ASSERT_TRUE(model->JointNameExists("joint")); + EXPECT_EQ("child_frame", model->JointByName("joint")->ChildLinkName()); + EXPECT_EQ("parent_link", model->JointByName("joint")->ParentLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE( + model->JointByName("joint")->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("child_link", resolvedLinkName); + EXPECT_TRUE( + model->JointByName("joint")->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("parent_link", resolvedLinkName); + + EXPECT_TRUE(model->JointByName("joint")->PoseRelativeTo().empty()); + EXPECT_EQ(Pose(0, 1, 0, 0, 0, 0), model->JointByName("joint")->RawPose()); + + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + + ASSERT_TRUE(model->FrameNameExists("child_frame")); + + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), + model->FrameByName("child_frame")->RawPose()); + + // Test ResolveFrame to get each link, joint and frame pose in model frame. + Pose pose; + EXPECT_TRUE( + model->LinkByName("parent_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose); + EXPECT_TRUE( + model->LinkByName("child_link")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 1, 10, 0, 0, 0), pose); + EXPECT_TRUE( + model->FrameByName("child_frame")-> + SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 10, 0, 0, 0), pose); + + // joint frame relative to parent and child links + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "child_link").empty()); + EXPECT_EQ(Pose(1, 1, 0, 0, 0, 0), pose); + EXPECT_TRUE( + model->JointByName("joint")-> + SemanticPose().Resolve(pose, "parent_link").empty()); + EXPECT_EQ(Pose(1, 1, 9, 0, 0, 0), pose); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { @@ -366,7 +560,7 @@ TEST(DOMJoint, LoadInvalidChild) EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( - "Child link with name[invalid] specified by joint with name[joint] not " + "Child frame with name[invalid] specified by joint with name[joint] not " "found")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR); EXPECT_NE(std::string::npos, @@ -397,15 +591,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].")); } diff --git a/test/sdf/joint_child_frame.sdf b/test/sdf/joint_child_frame.sdf new file mode 100644 index 000000000..2b2c900a2 --- /dev/null +++ b/test/sdf/joint_child_frame.sdf @@ -0,0 +1,25 @@ + + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 0 0 0 + parent_link + child_frame + + + diff --git a/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf b/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf new file mode 100644 index 000000000..d449f983f --- /dev/null +++ b/test/sdf/joint_invalid_resolved_parent_same_as_child.sdf @@ -0,0 +1,24 @@ + + + + + + + L1 + J2 + + 1 0 0 + + + + + J1 + L2 + + 0 1 0 + + + + + + diff --git a/test/sdf/joint_invalid_self_child.sdf b/test/sdf/joint_invalid_self_child.sdf new file mode 100644 index 000000000..7f4763340 --- /dev/null +++ b/test/sdf/joint_invalid_self_child.sdf @@ -0,0 +1,13 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + link + self + + + diff --git a/test/sdf/joint_invalid_self_parent.sdf b/test/sdf/joint_invalid_self_parent.sdf new file mode 100644 index 000000000..7dde303e0 --- /dev/null +++ b/test/sdf/joint_invalid_self_parent.sdf @@ -0,0 +1,13 @@ + + + + + 0 0 1 0 0 0 + + + 0 0 3 0 0 0 + self + link + + + diff --git a/test/sdf/joint_parent_frame.sdf b/test/sdf/joint_parent_frame.sdf new file mode 100644 index 000000000..2ad7afc8e --- /dev/null +++ b/test/sdf/joint_parent_frame.sdf @@ -0,0 +1,25 @@ + + + + + + 0 0 1 0 0 0 + + + 0 0 10 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 0 0 0 + parent_frame + child_link + + +