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

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

Hide file tree

Showing 9 changed files with 1,971 additions and 28 deletions.
diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc
@@ -26,6 +26,7 @@
 #include "sdf/SDFImpl.hh"
 #include "sdf/sdf_config.h"
 #include "test_config.h"
+#include "test_utils.hh"
 
 #ifdef _WIN32
   #define popen  _popen

diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc
@@ -19,6 +19,7 @@
 #include "sdf/parser.hh"
 #include "sdf/Element.hh"
 #include "test_config.h"
+#include "test_utils.hh"
 
 /////////////////////////////////////////////////
 TEST(Parser, initStringTrim)

diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -32,7 +32,9 @@
 #include <urdf_parser/urdf_parser.h>
 
 #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<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
@@ -2645,48 +2648,129 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference)
 void CreateSDF(TiXmlElement *_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 (!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 "
+                      << "<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());
+        }
 
-    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 "
+                        << "<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
+      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 <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