URDF->SDF handle links with no inertia or small mass

include/sdf/ParserConfig.hh

@@ -174,6 +174,25 @@ class SDFORMAT_VISIBLE ParserConfig
   /// \brief Get the preserveFixedJoint flag value.
   public: bool URDFPreserveFixedJoint() const;
 
+  /// \brief Set the minimum allowed URDF link mass when converting to SDF.
+  /// Default value as 1e-6.
+  public: void URDFSetMinimumAllowedLinkMass(double _minimumAllowedLinkMass);
+
+  /// \brief Get the minimum allowed URDF link mass when converting to SDF.
+  /// Default value as 1e-6.
+  public: double URDFMinimumAllowedLinkMass() const;
+
+  /// \brief Set the convertLinkWithNoMassToFrame flag value, which determines
+  /// whether to convert URDF links with small (equal to or less than
+  /// URDFMinimumAllowedLinkMass) or no mass to frames in SDF.
+  public: void URDFSetConvertLinkWithNoMassToFrame(
+      bool _convertLinkWithNoMassToFrame);
+
+  /// \brief Get the convertLinkWithNoMassToFrame flag value, which determines
+  /// whether to convert URDF links with small (equal to or less than
+  /// URDFMinimumAllowedLinkMass) or no mass to frames in SDF.
+  public: bool URDFConvertLinkWithNoMassToFrame() const;
+
   /// \brief Private data pointer.
   IGN_UTILS_IMPL_PTR(dataPtr)
 };

src/ParserConfig.cc

@@ -50,6 +50,13 @@ class sdf::ParserConfig::Implementation
   /// reading the SDF/URDF file.
   public: bool preserveFixedJoint = false;
 
+  /// \brief Minimum allowed URDF link mass when converting to SDF.
+  public: double minimumAllowedLinkMass = 1e-6;
+
+  /// \brief Flag to convert of links with small (equal to or less than
+  /// minimumAllowedLinkMass) or no mass in URDF to frames in SDF.
+  public: bool convertLinkWithNoMassToFrame = false;
+
   /// \brief Flag to use <include> tags within ToElement methods instead of
   /// the fully included model.
   public: bool toElementUseIncludeTag = true;
@@ -173,3 +180,28 @@ bool ParserConfig::URDFPreserveFixedJoint() const
 {
   return this->dataPtr->preserveFixedJoint;
 }
+
+/////////////////////////////////////////////////
+void ParserConfig::URDFSetMinimumAllowedLinkMass(double _minimumAllowedLinkMass)
+{
+  this->dataPtr->minimumAllowedLinkMass = _minimumAllowedLinkMass;
+}
+
+/////////////////////////////////////////////////
+double ParserConfig::URDFMinimumAllowedLinkMass() const
+{
+  return this->dataPtr->minimumAllowedLinkMass;
+}
+
+/////////////////////////////////////////////////
+void ParserConfig::URDFSetConvertLinkWithNoMassToFrame(
+  bool _convertLinkWithNoMassToFrame)
+{
+  this->dataPtr->convertLinkWithNoMassToFrame = _convertLinkWithNoMassToFrame;
+}
+
+/////////////////////////////////////////////////
+bool ParserConfig::URDFConvertLinkWithNoMassToFrame() const
+{
+  return this->dataPtr->convertLinkWithNoMassToFrame;
+}

src/parser_urdf.cc

@@ -150,12 +150,19 @@ void AddTransform(tinyxml2::XMLElement *_elem,
     const gz::math::Pose3d &_transform);
 
 /// create SDF from URDF link
-void CreateSDF(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link);
+void CreateSDF(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link,
+    const ParserConfig& _parserConfig);
 
 /// create SDF Link block based on URDF
 void CreateLink(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link,
                 const gz::math::Pose3d &_currentTransform);
 
+/// create SDF Frame block based on a URDF link with small (less than
+/// ParserConfig::URDFMinimumAllowedLinkMass) or no mass
+void CreateFrameFromLink(tinyxml2::XMLElement *_root,
+                         urdf::LinkConstSharedPtr _link,
+                         const gz::math::Pose3d &_currentTransform);
+
 /// reduced fixed joints:  apply appropriate frame updates in joint
 ///   inside urdf extensions when doing fixed joint reduction
 void ReduceSDFExtensionJointFrameReplace(
@@ -2661,40 +2668,68 @@ 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.
+               urdf::LinkConstSharedPtr _link,
+               const ParserConfig& _parserConfig)
+{
+  // Links with a mass of less than or equal to
+  // ParserConfig::URDFMinimumAllowedLinkMass will considered a link with zero
+  // mass.
+  // Links with zero mass or without an <inertial> block will be ignored, unless
+  // ParserConfig::URDFConvertLinkWithNoMassToFrame is true, in which case they
+  // will be converted into frames.
+  // 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)))
+      ((!_link->inertial) ||
+          gz::math::equal(_link->inertial->mass, 0.0,
+              _parserConfig.URDFMinimumAllowedLinkMass())) &&
+      !_parserConfig.URDFConvertLinkWithNoMassToFrame())
   {
+    std::stringstream inertiaIssue;
+    if (_link->inertial && gz::math::equal(_link->inertial->mass, 0.0,
+        _parserConfig.URDFMinimumAllowedLinkMass()))
+    {
+      inertiaIssue << "a mass value of less than or equal to "
+                    << _parserConfig.URDFMinimumAllowedLinkMass();
+    }
+    else
+    {
+      inertiaIssue << "no inertia defined";
+    }
+
     if (!_link->child_links.empty())
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, ["
-             << static_cast<int>(_link->child_links.size())
-             << "] children links ignored.\n";
+      sdfwarn << "urdf2sdf: link[" << _link->name
+              << "] has "
+              << inertiaIssue.str()
+              << ", ["
+              << static_cast<int>(_link->child_links.size())
+              << "] children links ignored.\n";
     }
 
     if (!_link->child_joints.empty())
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, ["
-             << static_cast<int>(_link->child_links.size())
-             << "] children joints ignored.\n";
+      sdfwarn << "urdf2sdf: link[" << _link->name
+              << "] has "
+              << inertiaIssue.str()
+              << ", ["
+              << static_cast<int>(_link->child_links.size())
+              << "] children joints ignored.\n";
     }
 
     if (_link->parent_joint)
     {
-      sdfdbg << "urdf2sdf: link[" << _link->name
-             << "] has no inertia, "
-             << "parent joint [" << _link->parent_joint->name
-             << "] ignored.\n";
+      sdfwarn << "urdf2sdf: link[" << _link->name
+              << "] has "
+              << inertiaIssue.str()
+              << ", parent joint [" << _link->parent_joint->name
+              << "] ignored.\n";
     }
 
-    sdfdbg << "urdf2sdf: link[" << _link->name
-           << "] has no inertia, not modeled in sdf\n";
+    sdfwarn << "urdf2sdf: link[" << _link->name
+            << "] has "
+            << inertiaIssue.str()
+            << ", not modeled in sdf\n";
     return;
   }
 
@@ -2704,13 +2739,23 @@ void CreateSDF(tinyxml2::XMLElement *_root,
       (!_link->parent_joint ||
        !FixedJointShouldBeReduced(_link->parent_joint)))
   {
-    CreateLink(_root, _link, gz::math::Pose3d::Zero);
+    if (((!_link->inertial) ||
+            gz::math::equal(_link->inertial->mass, 0.0,
+                _parserConfig.URDFMinimumAllowedLinkMass())) &&
+        _parserConfig.URDFConvertLinkWithNoMassToFrame())
+    {
+      CreateFrameFromLink(_root, _link, gz::math::Pose3d::Zero);
+    }
+    else
+    {
+      CreateLink(_root, _link, gz::math::Pose3d::Zero);
+    }
   }
 
   // recurse into children
   for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
   {
-    CreateSDF(_root, _link->child_links[i]);
+    CreateSDF(_root, _link->child_links[i], _parserConfig);
   }
 }
 
@@ -2793,6 +2838,43 @@ void CreateLink(tinyxml2::XMLElement *_root,
   _root->LinkEndChild(elem);
 }
 
+////////////////////////////////////////////////////////////////////////////////
+void CreateFrameFromLink(tinyxml2::XMLElement *_root,
+                         urdf::LinkConstSharedPtr _link,
+                         const gz::math::Pose3d &_currentTransform)
+{
+  // create new body
+  tinyxml2::XMLElement *elem = _root->GetDocument()->NewElement("frame");
+
+  // set body name
+  elem->SetAttribute("name", _link->name.c_str());
+
+  // compute global transform
+  gz::math::Pose3d localTransform;
+  // this is the transform from parent link to current _link
+  // this transform does not exist for the root link
+  if (_link->parent_joint)
+  {
+    tinyxml2::XMLElement *pose = _root->GetDocument()->NewElement("pose");
+    pose->SetAttribute("relative_to", _link->parent_joint->name.c_str());
+    elem->LinkEndChild(pose);
+  }
+  else
+  {
+    if (_currentTransform != gz::math::Pose3d::Zero)
+    {
+      // create origin tag for this element
+      AddTransform(elem, _currentTransform);
+    }
+  }
+
+  // make a <joint:...> block
+  CreateJoint(_root, _link, _currentTransform);
+
+  // add body to document
+  _root->LinkEndChild(elem);
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 void CreateCollisions(tinyxml2::XMLElement* _elem,
                       urdf::LinkConstSharedPtr _link)
@@ -3306,13 +3388,13 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr,
           child = rootLink->child_links.begin();
           child != rootLink->child_links.end(); ++child)
       {
-        CreateSDF(robot, (*child));
+        CreateSDF(robot, (*child), _config);
       }
     }
     else
     {
       // convert, starting from root link
-      CreateSDF(robot, rootLink);
+      CreateSDF(robot, rootLink, _config);
     }
 
     // insert the extensions without reference into <robot> root level