diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 854165f35..1b320fbf9 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -32,7 +32,9 @@ #include #include "sdf/parser_urdf.hh" +#include "sdf/Error.hh" #include "sdf/sdf.hh" +#include "sdf/Types.hh" #include "SDFExtension.hh" @@ -57,7 +59,8 @@ bool g_initialRobotPoseValid = false; std::set g_fixedJointsTransformedInRevoluteJoints; std::set g_fixedJointsTransformedInFixedJoints; const int g_outputDecimalPrecision = 16; - +const char kSdformatUrdfExtensionUrl[] = + "http://sdformat.org/tutorials?tut=sdformat_urdf_extensions"; /// \brief parser xml string into urdf::Vector3 /// \param[in] _key XML key where vector3 value might be @@ -2645,48 +2648,129 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference) void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link) { - // must have an block and cannot have zero mass. - // allow det(I) == zero, in the case of point mass geoms. + // Links without an block will be considered to have zero mass. + const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0; + + // link must have an block and cannot have zero mass, if its parent + // joint and none of its child joints are reduced. + // allow det(I) == zero, in the case of point mass geoms. // @todo: keyword "world" should be a constant defined somewhere else - if (_link->name != "world" && - ((!_link->inertial) || gz::math::equal(_link->inertial->mass, 0.0))) + if (_link->name != "world" && linkHasZeroMass) { - if (!_link->child_links.empty()) + Errors nonJointReductionErrors; + std::stringstream errorStream; + errorStream << "urdf2sdf: link[" << _link->name << "] has "; + if (!_link->inertial) { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children links ignored.\n"; + errorStream << "no block defined. "; } - - if (!_link->child_joints.empty()) + else { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, [" - << static_cast(_link->child_links.size()) - << "] children joints ignored.\n"; + errorStream << "a mass value of less than or equal to zero. "; } + errorStream << "Please ensure this link has a valid mass to prevent any " + << "missing links or joints in the resulting SDF model."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + + // if the parent joint is reduced, which resolves the massless issue of this + // link, no warnings will be emitted + bool parentJointReduced = _link->parent_joint && + FixedJointShouldBeReduced(_link->parent_joint) && + g_reduceFixedJoints; + + if (!parentJointReduced) + { + // check if joint lumping was turned off for the parent joint + if (_link->parent_joint) + { + if (_link->parent_joint->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed parent joint[" + << _link->parent_joint->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, could " + << "help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } - if (_link->parent_joint) - { - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, " - << "parent joint [" << _link->parent_joint->name - << "] ignored.\n"; - } + errorStream << "urdf2sdf: parent joint[" + << _link->parent_joint->name + << "] ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } - sdfdbg << "urdf2sdf: link[" << _link->name - << "] has no inertia, not modeled in sdf\n"; - return; + // check if joint lumping was turned off for child joints + if (!_link->child_joints.empty()) + { + for (const auto &cj : _link->child_joints) + { + // Lumping child joints occur before CreateSDF, so if it does occur + // this link would already contain the lumped mass from the child + // link. If a child joint is still fixed at this point, it means joint + // lumping has been disabled. + if (cj->type == urdf::Joint::FIXED) + { + errorStream << "urdf2sdf: allowing joint lumping by removing any " + << " or " + << "gazebo tag on fixed child joint[" + << cj->name + << "], as well as ensuring that " + << "ParserConfig::URDFPreserveFixedJoint is false, " + << "could help resolve this warning. See " + << std::string(kSdformatUrdfExtensionUrl) + << " for more information about this behavior."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + } + + errorStream << "urdf2sdf: [" + << static_cast(_link->child_joints.size()) + << "] child joints ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + errorStream << "urdf2sdf: [" + << static_cast(_link->child_links.size()) + << "] child links ignored."; + nonJointReductionErrors.emplace_back( + ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + errorStream.str(std::string()); + } + + // Emit all accumulated warnings and return + for (const auto &e : nonJointReductionErrors) + { + sdfwarn << e << '\n'; + } + errorStream << "urdf2sdf: link[" << _link->name + << "] is not modeled in sdf."; + sdfwarn << Error(ErrorCode::LINK_INERTIA_INVALID, errorStream.str()); + return; + } } - // create block for non fixed joint attached bodies + // create block for non fixed joint attached bodies that have mass if ((_link->getParent() && _link->getParent()->name == "world") || !g_reduceFixedJoints || (!_link->parent_joint || !FixedJointShouldBeReduced(_link->parent_joint))) { - CreateLink(_root, _link, gz::math::Pose3d::Zero); + if (!linkHasZeroMass) + { + CreateLink(_root, _link, gz::math::Pose3d::Zero); + } } // recurse into children diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc index 06e27c111..e3cfa0af5 100644 --- a/src/parser_urdf_TEST.cc +++ b/src/parser_urdf_TEST.cc @@ -21,6 +21,7 @@ #include "sdf/sdf.hh" #include "sdf/parser_urdf.hh" +#include "test_utils.hh" ///////////////////////////////////////////////// std::string getMinimalUrdfTxt() @@ -844,6 +845,991 @@ TEST(URDFParser, OutputPrecision) EXPECT_EQ("0", poseValues[5]); } +///////////////////////////////////////////////// +TEST(URDFParser, LinksWithMassIssuesIdentified) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // explicitly zero mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } + + // no block defined + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + } + + // negative mass + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link1] has a mass value of less than or equal to zero. Please " + "ensure this link has a valid mass to prevent any missing links or " + "joints in the resulting SDF model"); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithNoFixedJoints) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // joint lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 visual lumps into link1 + TiXmlElement *lumpedVisual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, lumpedVisual); + ASSERT_NE(nullptr, lumpedVisual->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_visual", + std::string(lumpedVisual->Attribute("name"))); + + // link2 collision lumps into link1 + TiXmlElement *lumpedCollision = + link->FirstChildElement("collision"); + ASSERT_NE(nullptr, lumpedCollision); + ASSERT_NE(nullptr, lumpedCollision->Attribute("name")); + EXPECT_EQ("link1_fixed_joint_lump__link2_collision", + std::string(lumpedCollision->Attribute("name"))); + + // joint1_2 converted into frame, attached to link1 + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link1", std::string(frame->Attribute("attached_to"))); + + // link2 converted into frame, attached to joint1_2 + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link2", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint1_2", std::string(frame->Attribute("attached_to"))); + + // joint2_3 will change to be relative to link1 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(joint->Attribute("name"))); + TiXmlElement *jointPose = joint->FirstChildElement("pose"); + ASSERT_NE(nullptr, jointPose); + ASSERT_NE(nullptr, jointPose->Attribute("relative_to")); + EXPECT_EQ("link1", std::string(jointPose->Attribute("relative_to"))); + + // link3 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link3", std::string(link->Attribute("name"))); + TiXmlElement *linkPose = link->FirstChildElement("pose"); + ASSERT_NE(nullptr, linkPose); + ASSERT_NE(nullptr, linkPose->Attribute("relative_to")); + EXPECT_EQ("joint2_3", std::string(linkPose->Attribute("relative_to"))); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // conversion fails + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // mass of link3 lumped into link2, link2 not converted to frame + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // link2 visual and collision remain + TiXmlElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_visual", std::string(visual->Attribute("name"))); + TiXmlElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_collision", std::string(collision->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // joint2_3 converted into a frame + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // turn off lumping with gazebo tag, conversion fails with warnings to suggest + // removing gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassIntermediateLinkWithAllFixedJointsButLumpingOff) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // turn off all lumping with gazebo tags, conversion fails with suggestions to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + + // no other frames and joints + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + TiXmlElement *joint = model->FirstChildElement("joint"); + EXPECT_EQ(nullptr, joint); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // lumping and reduction occurs, no warnings should be present + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision lumped into link2 + TiXmlElement *visual = link->FirstChildElement("visual"); + ASSERT_NE(nullptr, visual); + ASSERT_NE(nullptr, visual->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_visual", + std::string(visual->Attribute("name"))); + TiXmlElement *collision = link->FirstChildElement("collision"); + ASSERT_NE(nullptr, collision); + ASSERT_NE(nullptr, collision->Attribute("name")); + EXPECT_EQ("link2_fixed_joint_lump__link3_collision", + std::string(collision->Attribute("name"))); + + // joint2_3 converted into a frame + TiXmlElement *frame = model->FirstChildElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("link2", std::string(frame->Attribute("attached_to"))); + + // link3 converted into a frame + frame = frame->NextSiblingElement("frame"); + ASSERT_NE(nullptr, frame); + ASSERT_NE(nullptr, frame->Attribute("name")); + EXPECT_EQ("link3", std::string(frame->Attribute("name"))); + ASSERT_NE(nullptr, frame->Attribute("attached_to")); + EXPECT_EQ("joint2_3", std::string(frame->Attribute("attached_to"))); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string str = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + SDF_SUPPRESS_DEPRECATED_BEGIN + sdf::URDF2SDF parser; + SDF_SUPPRESS_DEPRECATED_END + TiXmlDocument sdfResult = parser.InitModelString(str); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + TiXmlElement *sdf = sdfResult.FirstChildElement("sdf"); + ASSERT_NE(nullptr, sdf); + TiXmlElement *model = sdf->FirstChildElement("model"); + ASSERT_NE(nullptr, model); + TiXmlElement *link = model->FirstChildElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link1", std::string(link->Attribute("name"))); + + // link2 + link = link->NextSiblingElement("link"); + ASSERT_NE(nullptr, link); + ASSERT_NE(nullptr, link->Attribute("name")); + EXPECT_EQ("link2", std::string(link->Attribute("name"))); + + // joint1_2 + TiXmlElement *joint = model->FirstChildElement("joint"); + ASSERT_NE(nullptr, joint); + ASSERT_NE(nullptr, joint->Attribute("name")); + EXPECT_EQ("joint1_2", std::string(joint->Attribute("name"))); + + // link3 visual and collision not lumped into link2 + TiXmlElement *visual = link->FirstChildElement("visual"); + EXPECT_EQ(nullptr, visual); + TiXmlElement *collision = link->FirstChildElement("collision"); + EXPECT_EQ(nullptr, collision); + + // joint2_3 ignored + joint = joint->NextSiblingElement("joint"); + EXPECT_EQ(nullptr, joint); + link = link->NextSiblingElement("link"); + EXPECT_EQ(nullptr, link); + TiXmlElement *frame = model->FirstChildElement("frame"); + EXPECT_EQ(nullptr, frame); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f0d7cce9c..762c76a56 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -41,6 +41,7 @@ set(tests unknown.cc urdf_gazebo_extensions.cc urdf_joint_parameters.cc + urdf_to_sdf.cc visual_dom.cc world_dom.cc ) diff --git a/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf new file mode 100644 index 000000000..3c30afbc2 --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_lumped_and_reduced.urdf @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/invalid_force_torque_sensor_massless_child_link.urdf b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf new file mode 100644 index 000000000..6e4a8093e --- /dev/null +++ b/test/integration/invalid_force_torque_sensor_massless_child_link.urdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 1 + 100.0 + 1 + + child + + + + + + diff --git a/test/integration/urdf_to_sdf.cc b/test/integration/urdf_to_sdf.cc new file mode 100644 index 000000000..377eb14a1 --- /dev/null +++ b/test/integration/urdf_to_sdf.cc @@ -0,0 +1,763 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include "sdf/sdf.hh" +#include "sdf/parser.hh" +#include "test_config.h" +#include "test_utils.hh" + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidBasicURDF) +{ + std::string urdfXml = R"( + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ValidContinuousJointedURDF) +{ + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_TRUE(model->LinkNameExists("link2")); + EXPECT_TRUE(model->JointNameExists("joint1_2")); +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedParentJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // joint lumping and reduction occurs + { + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + ASSERT_TRUE(errors.empty()); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // link2 visual and collision lumps into link1 + EXPECT_TRUE(link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + + // joint1_2 converted into frame, attached to link1 + EXPECT_TRUE(model->FrameNameExists("joint1_2")); + const sdf::Frame *frame = model->FrameByName("joint1_2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link1"), frame->AttachedTo()); + + // link2 converted into frame, attached to joint1_2 + EXPECT_TRUE(model->FrameNameExists("link2")); + frame = model->FrameByName("link2"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint1_2"), frame->AttachedTo()); + + // joint2_3 will change to be relative to link1 + EXPECT_TRUE(model->JointNameExists("joint2_3")); + const sdf::Joint *joint = model->JointByName("joint2_3"); + ASSERT_NE(nullptr, joint); + EXPECT_EQ(std::string("link1"), joint->ParentLinkName()); + EXPECT_EQ(std::string("link3"), joint->ChildLinkName()); + + // link3 + EXPECT_TRUE(model->LinkNameExists("link3")); + } + + // Disabling lumping using gazebo tags, fails with warnings suggesting to + // remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // no sdf errors, however we expect warnings and ignored joints and links + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint1_2], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + const sdf::Link *link = model->LinkByName("link1"); + ASSERT_NE(nullptr, link); + + // expect everything below joint1_2 to be ignored + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + + // expect no visual or collision from link2 lumped into link1 + EXPECT_FALSE( + link->VisualNameExists("link1_fixed_joint_lump__link2_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link1_fixed_joint_lump__link2_collision")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, ZeroMassIntermediateLinkWithFixedChildJoint) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs, mass of link3 lumped into link2 + // link3 and joint2_3 converted into frames + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // lumping and reduction occurs, no warnings or errors should be present + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // mass of link3 lumped into link2, link2 not converted to frame + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link2 visual and collision remain + EXPECT_TRUE(link->VisualNameExists("link2_visual")); + EXPECT_TRUE(link->CollisionNameExists("link2_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // turn off lumping with gazebo tag, conversion fails with dropped links and + // joints, and warnings to remove gazebo tags + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // Everything beneath joint1_2 is ignored, no sdf errors, but warnings + // expected with suggestion to remove gazebo tag + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint1_2] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed child joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning."); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child joints ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "[1] child links ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link2] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + EXPECT_FALSE(model->LinkNameExists("link2")); + EXPECT_FALSE(model->FrameNameExists("link2")); + EXPECT_FALSE(model->JointNameExists("joint1_2")); + EXPECT_FALSE(model->FrameNameExists("joint1_2")); + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +///////////////////////////////////////////////// +TEST(URDFParser, ZeroMassLeafLink) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // lumping and reduction occurs + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // lumping and reduction occurs, no warnings should be present + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision lumped into link2 + EXPECT_TRUE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_TRUE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("joint2_3")); + const sdf::Frame *frame = model->FrameByName("joint2_3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("link2"), frame->AttachedTo()); + + // link3 converted into a frame + EXPECT_TRUE(model->FrameNameExists("link3")); + frame = model->FrameByName("link3"); + ASSERT_NE(nullptr, frame); + EXPECT_EQ(std::string("joint2_3"), frame->AttachedTo()); + } + + // lumping and reduction turned off with gazebo tag, expect link3 and joint2_3 + // to be ignored and warnings suggesting to remove gazebo tag + { + // clear the contents of the buffer + buffer.str(""); + + std::string urdfXml = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + )"; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(urdfXml); + + // joint2_3 and link3 will be ignored, and warnings suggesting to remove + // gazebo tags + ASSERT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "allowing joint lumping by removing any or " + " gazebo tag on fixed parent joint[joint2_3], as " + "well as ensuring that ParserConfig::URDFPreserveFixedJoint is false, " + "could help resolve this warning"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint2_3] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link3] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("link1")); + + // link2 + const sdf::Link *link = model->LinkByName("link2"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // joint1_2 + EXPECT_TRUE(model->JointNameExists("joint1_2")); + + // link3 visual and collision not lumped into link2 + EXPECT_FALSE( + link->VisualNameExists("link2_fixed_joint_lump__link3_visual")); + EXPECT_FALSE( + link->CollisionNameExists("link2_fixed_joint_lump__link3_collision")); + + // joint2_3 and link3 ignored + EXPECT_FALSE(model->LinkNameExists("link3")); + EXPECT_FALSE(model->FrameNameExists("link3")); + EXPECT_FALSE(model->JointNameExists("joint2_3")); + EXPECT_FALSE(model->FrameNameExists("joint2_3")); + } +} + +////////////////////////////////////////////////// +TEST(URDF2SDF, URDFConvertForceTorqueSensorModels) +{ + // Redirect sdfwarn output + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); +#ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); +#endif + + // valid force torque sensor + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile("integration", "force_torque_sensor.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + EXPECT_TRUE(errors.empty()); + } + + // invalid force torque sensor, with massless child link, fixed parent joint, + // lumping and reduction occurs, no errors or warning will be emitted, but + // force torque sensor won't be able to attach to joint-converted frame, this + // is the case where users need to be wary about + // TODO(aaronchongth): To provide warning when joint is dropped/converted + // during lumping and reduction + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_lumped_and_reduced.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + + // lumping and reduction occurs, we expect no warnings or errors + EXPECT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::notContains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->FrameNameExists("link_1")); + EXPECT_TRUE(model->FrameNameExists("joint_1")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + } + + // invalid force torque sensor, with massless child link, revolute parent + // joint, there will be warnings with joint_1 and link_1 ignored + { + // clear the contents of the buffer + buffer.str(""); + + const std::string sdfTestFile = + sdf::testing::TestFile( + "integration", + "invalid_force_torque_sensor_massless_child_link.urdf"); + sdf::Root root; + sdf::Errors errors = root.Load(sdfTestFile); + + // lumping and reduction does not occur, joint_1 and link_1 ignored + EXPECT_TRUE(errors.empty()); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] has no block defined. Please ensure this link " + "has a valid mass to prevent any missing links or joints in the " + "resulting SDF model"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "parent joint[joint_1] ignored"); + EXPECT_PRED2(sdf::testing::contains, buffer.str(), + "link[link_1] is not modeled in sdf"); + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_TRUE(model->LinkNameExists("base_link")); + EXPECT_TRUE(model->LinkNameExists("link_2")); + EXPECT_TRUE(model->JointNameExists("joint_2")); + EXPECT_FALSE(model->FrameNameExists("link_1")); + EXPECT_FALSE(model->LinkNameExists("link_1")); + EXPECT_FALSE(model->FrameNameExists("joint_1")); + EXPECT_FALSE(model->JointNameExists("joint_1")); + } +} diff --git a/test/test_utils.hh b/test/test_utils.hh index 150f70b60..44451d980 100644 --- a/test/test_utils.hh +++ b/test/test_utils.hh @@ -104,6 +104,18 @@ class RedirectConsoleStream private: sdf::Console::ConsoleStream oldStream; }; +/// \brief Checks that string _a contains string _b +inline bool contains(const std::string &_a, const std::string &_b) +{ + return _a.find(_b) != std::string::npos; +} + +/// \brief Check that string _a does not contain string _b +inline bool notContains(const std::string &_a, const std::string &_b) +{ + return !contains(_a, _b);; +} + } // namespace testing } // namespace sdf