From 2110484a86937908931627baa6de7092a8488dd8 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 24 Nov 2025 22:10:24 +0100 Subject: [PATCH 1/6] Quaternion in urdf (PR123 new attempt) (#194) Signed-off-by: Guillaume Doisy --- urdf_parser/src/pose.cpp | 18 ++++++++++++++++++ xsd/urdf.xsd | 1 + 2 files changed, 19 insertions(+) diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index a4a27c75..73eb9373 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -107,6 +107,13 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml) } const char* rpy_str = xml->Attribute("rpy"); + const char* quat_str = xml->Attribute("quat_xyzw"); + 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 +124,17 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml) return false; } } + + if (quat_str != NULL) + { + try { + pose.rotation.initQuaternion(quat_str); + } + catch (ParseError &e) { + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + } } return true; } diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index 3f2e67f4..086afa85 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -27,6 +27,7 @@ + From 25e67400ee84c7765bd2c3b275a70bfcbbdb4581 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 10 Dec 2025 00:11:14 -0800 Subject: [PATCH 2/6] Require version 2.0.1 of urdfdom_headers Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f6fff248..1897782a 100644 --- a/CMakeLists.txt +++ b/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) From 02326f7dbead37257cac2c9387f9c421d41a257e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 10 Dec 2025 00:12:12 -0800 Subject: [PATCH 3/6] Increment version to 1.1 in schema Update version in xml comment and add version attribute to xs:schema tag. Signed-off-by: Steve Peters --- xsd/urdf.xsd | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index 086afa85..93b00a3d 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -1,7 +1,7 @@ From bdc5bd37c047a4c47638b2902b3b38060be336a3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 10 Dec 2025 00:59:28 -0800 Subject: [PATCH 4/6] urdf_parser: allow schema version 1.1 This adds URDFVersion comparison helpers and updates the parser to allow parsing version 1.1. Signed-off-by: Steve Peters --- urdf_parser/include/urdf_parser/urdf_parser.h | 22 +++++++++++++++++++ urdf_parser/src/model.cpp | 4 ++-- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 8e32b0a6..906e303f 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -99,6 +99,28 @@ class URDFVersion final 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_; diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 6d6407f6..6b9dc90a 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -125,9 +125,9 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) 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"); } } catch (const std::runtime_error & err) From f00aebab817dc4c4416835a16cf930eaa198ccca Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 10 Dec 2025 01:04:54 -0800 Subject: [PATCH 5/6] Pass URDFVersion to parsing functions * Add URDFVersion constructor that accepts two integers * Create URDFVersion object in correct scope to pass to parsing functions * Pass URDFVersion argument to parsing functions Signed-off-by: Steve Peters --- urdf_parser/include/urdf_parser/urdf_parser.h | 4 ++++ urdf_parser/src/joint.cpp | 5 ++-- urdf_parser/src/link.cpp | 24 +++++++++++-------- urdf_parser/src/model.cpp | 17 +++++++++---- urdf_parser/src/pose.cpp | 3 ++- urdf_parser/src/pose.hpp | 5 +++- 6 files changed, 40 insertions(+), 18 deletions(-) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 906e303f..0e725b44 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -94,6 +94,10 @@ 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; diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 4269419a..42397528 100644 --- a/urdf_parser/src/joint.cpp +++ b/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()); diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index ab83b5d3..1916608d 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -266,7 +266,8 @@ 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(); @@ -274,7 +275,7 @@ bool parseInertial(Inertial &i, tinyxml2::XMLElement *config) 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); } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 6b9dc90a..ebaa103b 100644 --- a/urdf_parser/src/model.cpp +++ b/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,6 +124,10 @@ 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")); @@ -129,6 +135,8 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) { 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) { @@ -136,6 +144,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) 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)) { diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 73eb9373..47c6c83f 100644 --- a/urdf_parser/src/pose.cpp +++ b/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) diff --git a/urdf_parser/src/pose.hpp b/urdf_parser/src/pose.hpp index cadc1832..38e05192 100644 --- a/urdf_parser/src/pose.hpp +++ b/urdf_parser/src/pose.hpp @@ -37,8 +37,11 @@ #include #include +#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); } From 60efee31d7b9de5ef8234d6afb6136571d28a0b2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 10 Dec 2025 01:09:56 -0800 Subject: [PATCH 6/6] Only parse quat_xyzw with at least version 1.1 Update logic in parsePoseInternal to ignore quat_xyzw if specified version is not new enough, but print a warning message if any are detected. Signed-off-by: Steve Peters --- urdf_parser/src/pose.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 47c6c83f..a1c4b5e6 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -109,8 +109,10 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml, const char* rpy_str = xml->Attribute("rpy"); const char* quat_str = xml->Attribute("quat_xyzw"); - if (rpy_str != NULL && quat_str != NULL) - { + 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; } @@ -126,8 +128,7 @@ bool parsePoseInternal(Pose &pose, tinyxml2::XMLElement* xml, } } - if (quat_str != NULL) - { + if (version.at_least(1, 1) && quat_str != NULL) { try { pose.rotation.initQuaternion(quat_str); }