Support quaternions in URDF 1.1

CMakeLists.txt

@@ -47,7 +47,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 
 find_package(TinyXML2 REQUIRED)
 
-find_package(urdfdom_headers REQUIRED)
+find_package(urdfdom_headers 2.0.1 REQUIRED)
 find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it.
 find_package(console_bridge REQUIRED)
 

urdf_parser/include/urdf_parser/urdf_parser.h

@@ -94,11 +94,37 @@ class URDFVersion final
     }
   }
 
+  explicit URDFVersion(uint32_t major, uint32_t minor)
+    : major_(major), minor_(minor)
+  {}
+
   bool equal(uint32_t maj, uint32_t min)
   {
     return this->major_ == maj && this->minor_ == min;
   }
 
+  // equivalent to greater or equal >=
+  bool at_least(uint32_t maj, uint32_t min) const
+  {
+    return this->major_ > maj || (this->major_ == maj && this->minor_ >= min);
+  }
+
+  // equivalent to lesser or equal <=
+  bool at_most(uint32_t maj, uint32_t min) const
+  {
+    return this->major_ < maj || (this->major_ == maj && this->minor_ <= min);
+  }
+
+  bool greater_than(uint32_t maj, uint32_t min) const
+  {
+    return this->major_ > maj || (this->major_ == maj && this->minor_ > min);
+  }
+
+  bool less_than(uint32_t maj, uint32_t min) const
+  {
+    return this->major_ < maj || (this->major_ == maj && this->minor_ < min);
+  }
+
   uint32_t getMajor() const
   {
     return major_;

urdf_parser/src/joint.cpp

@@ -334,7 +334,8 @@ bool parseJointMimic(JointMimic &jm, tinyxml2::XMLElement* config)
   return true;
 }
 
-bool parseJoint(Joint &joint, tinyxml2::XMLElement* config)
+bool parseJoint(Joint &joint, tinyxml2::XMLElement* config,
+                const urdf_export_helpers::URDFVersion version)
 {
   joint.clear();
 
@@ -356,7 +357,7 @@ bool parseJoint(Joint &joint, tinyxml2::XMLElement* config)
   }
   else
   {
-    if (!parsePoseInternal(joint.parent_to_joint_origin_transform, origin_xml))
+    if (!parsePoseInternal(joint.parent_to_joint_origin_transform, origin_xml, version))
     {
       joint.parent_to_joint_origin_transform.clear();
       CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str());

urdf_parser/src/link.cpp

@@ -266,15 +266,16 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g)
   return GeometrySharedPtr();
 }
 
-bool parseInertial(Inertial &i, tinyxml2::XMLElement *config)
+bool parseInertial(Inertial &i, tinyxml2::XMLElement *config,
+                   const urdf_export_helpers::URDFVersion version)
 {
   i.clear();
 
   // Origin
   tinyxml2::XMLElement *o = config->FirstChildElement("origin");
   if (o)
   {
-    if (!parsePoseInternal(i.origin, o))
+    if (!parsePoseInternal(i.origin, o, version))
       return false;
   }
 
@@ -346,14 +347,15 @@ bool parseInertial(Inertial &i, tinyxml2::XMLElement *config)
   return true;
 }
 
-bool parseVisual(Visual &vis, tinyxml2::XMLElement *config)
+bool parseVisual(Visual &vis, tinyxml2::XMLElement *config,
+                 const urdf_export_helpers::URDFVersion version)
 {
   vis.clear();
 
   // Origin
   tinyxml2::XMLElement *o = config->FirstChildElement("origin");
   if (o) {
-    if (!parsePoseInternal(vis.origin, o))
+    if (!parsePoseInternal(vis.origin, o, version))
       return false;
   }
 
@@ -388,14 +390,15 @@ bool parseVisual(Visual &vis, tinyxml2::XMLElement *config)
   return true;
 }
 
-bool parseCollision(Collision &col, tinyxml2::XMLElement* config)
+bool parseCollision(Collision &col, tinyxml2::XMLElement* config,
+                    const urdf_export_helpers::URDFVersion version)
 {
   col.clear();
 
   // Origin
   tinyxml2::XMLElement *o = config->FirstChildElement("origin");
   if (o) {
-    if (!parsePoseInternal(col.origin, o))
+    if (!parsePoseInternal(col.origin, o, version))
       return false;
   }
 
@@ -412,7 +415,8 @@ bool parseCollision(Collision &col, tinyxml2::XMLElement* config)
   return true;
 }
 
-bool parseLink(Link &link, tinyxml2::XMLElement* config)
+bool parseLink(Link &link, tinyxml2::XMLElement* config,
+               const urdf_export_helpers::URDFVersion version)
 {
 
   link.clear();
@@ -430,7 +434,7 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config)
   if (i)
   {
     link.inertial.reset(new Inertial());
-    if (!parseInertial(*link.inertial, i))
+    if (!parseInertial(*link.inertial, i, version))
     {
       CONSOLE_BRIDGE_logError("Could not parse inertial element for Link [%s]", link.name.c_str());
       return false;
@@ -443,7 +447,7 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config)
 
     VisualSharedPtr vis;
     vis.reset(new Visual());
-    if (parseVisual(*vis, vis_xml))
+    if (parseVisual(*vis, vis_xml, version))
     {
       link.visual_array.push_back(vis);
     }
@@ -465,7 +469,7 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config)
   {
     CollisionSharedPtr col;
     col.reset(new Collision());
-    if (parseCollision(*col, col_xml))
+    if (parseCollision(*col, col_xml, version))
     {
       link.collision_array.push_back(col);
     }

urdf_parser/src/model.cpp

@@ -45,8 +45,10 @@
 namespace urdf{
 
 bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_name_is_ok);
-bool parseLink(Link &link, tinyxml2::XMLElement *config);
-bool parseJoint(Joint &joint, tinyxml2::XMLElement *config);
+bool parseLink(Link &link, tinyxml2::XMLElement *config,
+               const urdf_export_helpers::URDFVersion version);
+bool parseJoint(Joint &joint, tinyxml2::XMLElement *config,
+                const urdf_export_helpers::URDFVersion version);
 
 ModelInterfaceSharedPtr  parseURDFFile(const std::string &path)
 {
@@ -122,20 +124,27 @@ ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
   }
   model->name_ = std::string(name);
 
+  // Creating URDFVersion from a string can throw exceptions, so create variables here to store the
+  // major and minor versions and then reconstruct a URDFVersion in this scope.
+  uint32_t major_version = 1;
+  uint32_t minor_version = 0;
   try
   {
     urdf_export_helpers::URDFVersion version(robot_xml->Attribute("version"));
-    if (!version.equal(1, 0))
+    if (version.less_than(1, 0) || version.greater_than(1, 1))
     {
-      throw std::runtime_error("Invalid 'version' specified; only version 1.0 is currently supported");
+      throw std::runtime_error("Invalid 'version' specified; versions 1.0 to 1.1 are currently supported");
     }
+    major_version = version.getMajor();
+    minor_version = version.getMajor();
   }
   catch (const std::runtime_error & err)
   {
     CONSOLE_BRIDGE_logError(err.what());
     model.reset();
     return model;
   }
+  urdf_export_helpers::URDFVersion version(major_version, minor_version);
 
   // Get all Material elements
   for (tinyxml2::XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
@@ -173,7 +182,7 @@ ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
     link.reset(new Link);
 
     try {
-      parseLink(*link, link_xml);
+      parseLink(*link, link_xml, version);
       if (model->getLink(link->name))
       {
         CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
@@ -215,7 +224,7 @@ ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
     JointSharedPtr joint;
     joint.reset(new Joint);
 
-    if (parseJoint(*joint, joint_xml))
+    if (parseJoint(*joint, joint_xml, version))
     {
       if (model->getJoint(joint->name))
       {

urdf_parser/src/pose.cpp

@@ -89,7 +89,8 @@ std::string values2str(double d)
 
 namespace urdf{
 
-bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml)
+bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml,
+                       const urdf_export_helpers::URDFVersion version)
 {
   pose.clear();
   if (xml)
@@ -107,6 +108,15 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml)
     }
 
     const char* rpy_str = xml->Attribute("rpy");
+    const char* quat_str = xml->Attribute("quat_xyzw");
+    if (version.less_than(1, 1) && quat_str != NULL) {
+      CONSOLE_BRIDGE_logWarn("Ignoring quat_xyzw attribute requiring URDF version 1.1 since specified version is 1.0.");
+    }
+    else if (rpy_str != NULL && quat_str != NULL) {
+      CONSOLE_BRIDGE_logError("Both rpy and quat_xyzw orientations are defined. Use either one or the other.");
+      return false;
+    }
+
     if (rpy_str != NULL)
     {
       try {
@@ -117,6 +127,16 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml)
         return false;
       }
     }
+
+    if (version.at_least(1, 1) && quat_str != NULL) {
+      try {
+        pose.rotation.initQuaternion(quat_str);
+      }
+      catch (ParseError &e) {
+        CONSOLE_BRIDGE_logError(e.what());
+        return false;
+      }
+    }
   }
   return true;
 }

urdf_parser/src/pose.hpp

@@ -37,8 +37,11 @@
 #include <urdf_model/pose.h>
 #include <tinyxml2.h>
 
+#include "urdf_parser/urdf_parser.h"
+
 namespace urdf {
 
-URDFDOM_DLLAPI bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml);
+URDFDOM_DLLAPI bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml,
+                                      const urdf_export_helpers::URDFVersion version);
 
 }

xsd/urdf.xsd

@@ -1,14 +1,15 @@
 <?xml version="1.0" ?>
 <!--
 
-   XML Schema for URDF v1.0
+   XML Schema for URDF v1.1
 
    This is a proposal XML Schema to validate the original URDF file
    format. It supports PR2 extensions (transmission) but not the
    Gazebo ones.
 
   -->
 <xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema"
+	   version="1.1"
 	   elementFormDefault="unqualified">
 
   <!-- data type definitions -->
@@ -27,6 +28,7 @@
   <xs:complexType name="pose">
     <xs:attribute name="xyz" type="xs:string" default="0 0 0" />
     <xs:attribute name="rpy" type="xs:string" default="0 0 0" />
+    <xs:attribute name="quat_xyzw" type="xs:string" default="0 0 0 1" />
   </xs:complexType>
 
   <!-- pose node type -->