URDF->SDF handle links with no inertia or small mass (gazebosim#1238)

* Adding tests that catch warnings when urdf have no inertia element, or if mass is too small Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Promote link inertia and mass related urdf2sdf sdfdbg to sdfwarn with more verbose messages Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added flag for converting urdf links with small or no mass to frames Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Converts urdf links with small or no mass to frames, added tests Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Adding warning when conversion happens, added tests for small masses, being explicit about epsilon in equal Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix cpplint errors Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Adding integration test Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix lint Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Use camelCase Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added URDFMinimumAllowedLinkMass Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix tests expecting warnings when converting to frames Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Adding inline contains and notContains to test_utils Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Using RedirectConsoleStream and ScopeExit Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Refactor to use Root::LoadSdfString Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Removing debug message when converted frame is from a root link Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added attached_to for frames during conversion, using < instead of math::equals Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Update brief of URDFSetConvertLinkWithNoMassToFrame to mention case of no inertial frame Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Remove stale commentted code Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Update comment about the errors we are expecting Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Convert to frame by default, remove minimal mass option, refactor implementation to handle lumping, modified unit tests Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Only convert to frame when parent joint is fixed, attaches and transforms pose to parent link, leaves out the fixed joint Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Rephrased conversion error, modified unit tests Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * prints zero mass errors as well when conversion to frame fails Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * integration tests that mimic the unit tests Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added integration test with valid and invalid use of force torque sensor where massless child links occur Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix lint Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Fix joint reduction logic, more specific error messages, more targeted unit tests for each case Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Convert joints to frames when converting links, attach them to converted links Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Integration tests with child fixed links as well Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Change sdferr to sdfwarn, no way to use ParserConfig::WarningsPolicy for now Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * sdferr to sdfwarn for the case where conversion to frame is attempted Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Removing case within converting to frame, where parent joint turns into revolute joint, that is not covered Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Reduced scope of implementation, more specific warning messages Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Remove mention of parser config in warning, since that is not typically used by workflows Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Integration tests revisited and modified, removed printouts Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Test case showing ParserConfig::URDFSetPreserveFixedJoint(true) overrides gazebo tags for joint lumping, warnings with more information and placeholder url Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Use sdf::testing::contains instead of local contains functions in testing (gazebosim#1251) Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Remove unused InitSDF, added TODO for warning when joints are converted/dropped Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Cleaned up if else cases Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Using << operator of Errors Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Replacing placeholder url with expected URL for the documentation from sdf_tutorials#88 Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * URL to tutorial as a constant, removing checking URL from test, just in case links change, we are not testing for that anyway Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Refactored fixed child joint logic as it is never reached Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Reduce number of if statements, renaming to only check if parent joint is reduced Signed-off-by: Aaron Chong <aaronchongth@gmail.com> --------- Signed-off-by: Aaron Chong <aaronchongth@gmail.com>
azeey · Mar 22, 2023 · 6ffe669 · 6ffe669
1 parent d9d5d75
commit 6ffe669
Show file tree

Hide file tree

Showing 11 changed files with 2,859 additions and 170 deletions.
diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc
diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc
@@ -24,6 +24,7 @@
 #include "sdf/Console.hh"
 #include "sdf/Filesystem.hh"
 #include "test_config.h"
+#include "test_utils.hh"
 
 /////////////////////////////////////////////////
 TEST(Parser, initStringTrim)
@@ -277,13 +278,6 @@ TEST(Parser, NameUniqueness)
   }
 }
 
-/////////////////////////////////////////////////
-/// Check that _a contains _b
-static bool contains(const std::string &_a, const std::string &_b)
-{
-  return _a.find(_b) != std::string::npos;
-}
-
 /////////////////////////////////////////////////
 TEST(Parser, SyntaxErrorInValues)
 {
@@ -301,10 +295,11 @@ TEST(Parser, SyntaxErrorInValues)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Unable to set value [bad 0 0 0 0 0] for key [pose]");
-    EXPECT_PRED2(contains, buffer.str(), "bad_syntax_pose.sdf:L5");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "bad_syntax_pose.sdf:L5");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/pose:");
   }
   {
@@ -315,10 +310,11 @@ TEST(Parser, SyntaxErrorInValues)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Unable to set value [bad ] for key[linear]");
-    EXPECT_PRED2(contains, buffer.str(), "bad_syntax_double.sdf:L7");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "bad_syntax_double.sdf:L7");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/model[@name=\"robot1\"]/"
                  "link[@name=\"link\"]/velocity_decay/linear:");
   }
@@ -330,10 +326,11 @@ TEST(Parser, SyntaxErrorInValues)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Unable to set value [0 1 bad ] for key[gravity]");
-    EXPECT_PRED2(contains, buffer.str(), "bad_syntax_vector.sdf:L4");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "bad_syntax_vector.sdf:L4");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/gravity:");
   }
 
@@ -363,15 +360,15 @@ TEST(Parser, MissingRequiredAttributesErrors)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Error Code " +
                  std::to_string(
                     static_cast<int>(sdf::ErrorCode::ATTRIBUTE_MISSING)));
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Required attribute[name] in element[link] is not specified "
                  "in SDF.");
-    EXPECT_PRED2(contains, buffer.str(), "box_bad_test.world:L6");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(), "box_bad_test.world:L6");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/model[@name=\"box\"]/link:");
   }
 
@@ -401,14 +398,15 @@ TEST(Parser, IncludesErrors)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Error Code " +
                  std::to_string(
                     static_cast<int>(sdf::ErrorCode::ATTRIBUTE_MISSING)));
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "<include> element missing 'uri' attribute");
-    EXPECT_PRED2(contains, buffer.str(), "includes_missing_uri.sdf:L5");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "includes_missing_uri.sdf:L5");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/include[0]:");
   }
   {
@@ -421,14 +419,15 @@ TEST(Parser, IncludesErrors)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Error Code " +
                  std::to_string(
                     static_cast<int>(sdf::ErrorCode::URI_LOOKUP)));
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Unable to find uri[missing_model]");
-    EXPECT_PRED2(contains, buffer.str(), "includes_missing_model.sdf:L6");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "includes_missing_model.sdf:L6");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/include[0]/uri:");
   }
   {
@@ -446,15 +445,16 @@ TEST(Parser, IncludesErrors)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Error Code " +
                  std::to_string(static_cast<int>(sdf::ErrorCode::URI_LOOKUP)));
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Unable to resolve uri[box_missing_config]");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "since it does not contain a model.config");
-    EXPECT_PRED2(contains, buffer.str(), "includes_model_without_sdf.sdf:L6");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "includes_model_without_sdf.sdf:L6");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/include[0]/uri:");
   }
   {
@@ -472,15 +472,16 @@ TEST(Parser, IncludesErrors)
     sdf::init(sdf);
 
     sdf::readFile(path, sdf);
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Error Code " +
                  std::to_string(
                     static_cast<int>(sdf::ErrorCode::ELEMENT_MISSING)));
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "Failed to find top level <model> / <actor> / <light> for "
                  "<include>\n");
-    EXPECT_PRED2(contains, buffer.str(), "includes_without_top_level.sdf:L6");
-    EXPECT_PRED2(contains, buffer.str(),
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
+                 "includes_without_top_level.sdf:L6");
+    EXPECT_PRED2(sdf::testing::contains, buffer.str(),
                  "/sdf/world[@name=\"default\"]/include[0]/uri:");
   }
 

diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -31,7 +31,9 @@
 #include <urdf_model/link.h>
 #include <urdf_parser/urdf_parser.h>
 
+#include "sdf/Error.hh"
 #include "sdf/sdf.hh"
+#include "sdf/Types.hh"
 
 #include "XmlUtils.hh"
 #include "SDFExtension.hh"
@@ -61,7 +63,8 @@ bool g_initialRobotPoseValid = false;
 std::set<std::string> g_fixedJointsTransformedInRevoluteJoints;
 std::set<std::string> 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
@@ -2663,48 +2666,126 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference)
 void CreateSDF(tinyxml2::XMLElement *_root,
                urdf::LinkConstSharedPtr _link)
 {
-  // must have an <inertial> block and cannot have zero mass.
-  //  allow det(I) == zero, in the case of point mass geoms.
+  // Links without an <inertial> block will be considered to have zero mass.
+  const bool linkHasZeroMass = !_link->inertial || _link->inertial->mass <= 0;
+
+  // link must have an <inertial> 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<int>(_link->child_links.size())
-             << "] children links ignored.\n";
+      errorStream << "no <inertial> block defined. ";
     }
-
-    if (!_link->child_joints.empty())
+    else
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, ["
-             << static_cast<int>(_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 (_link->parent_joint)
+    if (!parentJointReduced)
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, "
-             << "parent joint [" << _link->parent_joint->name
-             << "] ignored.\n";
-    }
+      // 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 "
+                      << "<disableFixedJointLumping> or <preserveFixedJoint> "
+                      << "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());
+        }
 
-    sdfdbg << "urdf2sdf: link[" << _link->name
-           << "] has no inertia, not modeled in sdf\n";
-    return;
+        errorStream << "urdf2sdf: parent joint["
+                    << _link->parent_joint->name
+                    << "] ignored.";
+        nonJointReductionErrors.emplace_back(
+            ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
+        errorStream.str(std::string());
+      }
+
+      // 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 "
+                        << "<disableFixedJointLumping> or <preserveFixedJoint> "
+                        << "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<int>(_link->child_joints.size())
+                    << "] child joints ignored.";
+        nonJointReductionErrors.emplace_back(
+            ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
+        errorStream.str(std::string());
+        errorStream << "urdf2sdf: ["
+                    << static_cast<int>(_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
+      sdfwarn << nonJointReductionErrors;
+      errorStream << "urdf2sdf: link[" << _link->name
+                  << "] is not modeled in sdf.";
+      sdfwarn << Error(ErrorCode::LINK_INERTIA_INVALID, errorStream.str());
+      return;
+    }
   }
 
-  // create <body:...> block for non fixed joint attached bodies
+  // create <body:...> 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