From 4c8716dab5aa4cf54e6a79c04690f020e5ce249e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Jan 2026 23:46:02 +0100 Subject: [PATCH 1/5] Add support for capsule geometry type Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 59 ++++++++++++++++++++++++++++++++++++++-- xsd/urdf.xsd | 7 +++++ 2 files changed, 63 insertions(+), 3 deletions(-) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 1916608d..086783ca 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -216,7 +216,47 @@ bool parseMesh(Mesh &m, tinyxml2::XMLElement *c) return true; } -GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g) +bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) +{ + c.clear(); + + c.type = Geometry::CAPSULE; + if (!elem->Attribute("length") || + !elem->Attribute("radius")) + { + CONSOLE_BRIDGE_logError("Capsule shape must have both length and radius attributes"); + return false; + } + + try { + c.length = strToDouble(elem->Attribute("length")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "length [" << elem->Attribute("length") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + try { + c.radius = strToDouble(elem->Attribute("radius")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "radius [" << elem->Attribute("radius") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) + { + CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); + return false; + } + + return true; +} + +GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g, + const urdf_export_helpers::URDFVersion version) { GeometrySharedPtr geom; if (!g) return geom; @@ -257,6 +297,19 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g) if (parseMesh(*m, shape)) return geom; } + else if (type_name == "capsule") + { + if (version.less_than(1, 1)) { + CONSOLE_BRIDGE_logWarn("Ignoring capsule attribute minimum required version URDF version 1.1 since specified version is 1.0."); + return GeometrySharedPtr(); + } + else { + Capsule *c = new Capsule(); + geom.reset(c); + if (parseCapsule(*c, shape)) + return geom; + } + } else { CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); @@ -361,7 +414,7 @@ bool parseVisual(Visual &vis, tinyxml2::XMLElement *config, // Geometry tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); - vis.geometry = parseGeometry(geom); + vis.geometry = parseGeometry(geom, version); if (!vis.geometry) return false; @@ -404,7 +457,7 @@ bool parseCollision(Collision &col, tinyxml2::XMLElement* config, // Geometry tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); - col.geometry = parseGeometry(geom); + col.geometry = parseGeometry(geom, version); if (!col.geometry) return false; diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index 93b00a3d..15c18b99 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -98,6 +98,12 @@ + + + + + + @@ -105,6 +111,7 @@ + From 7f97ab8397b1d31305c4f5d6886679de7dbea289 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 30 Jan 2026 00:06:18 +0100 Subject: [PATCH 2/5] Add documentation about versioning Signed-off-by: Sai Kishor Kothakota --- README.md | 96 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/README.md b/README.md index 0c755b88..74de4a0d 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,102 @@ For example: wget https://raw.github.com/ros-gbp/urdfdom-release/debian/hydro/precise/urdfdom/package.xml ``` +### URDF Versioning + +The URDF format supports versioning to allow for format evolution while maintaining compatibility. The version is specified in the `` tag using the `version` attribute in the format `x.y` (e.g., `"1.0"`, `"1.1"`). + +#### Supported Versions + +| Version | Status | Description | +|---------|--------|-------------| +| **1.0** | Default | The original URDF specification. If no version attribute is specified, version 1.0 is assumed. | +| **1.1** | Supported | Adds quaternion orientation support via the `quat_xyzw` attribute and capsule geometry. | + +#### Version 1.0 (Default) + +Version 1.0 is the default URDF version. When the `version` attribute is omitted from the `` tag, the parser assumes version 1.0. + +Features supported in version 1.0: +- Robot model definition (links, joints, materials) +- Visual and collision geometry (box, cylinder, sphere, mesh) +- Joint types: revolute, continuous, prismatic, fixed, floating, planar +- Pose specification using `xyz` and `rpy` (roll-pitch-yaw) attributes +- Transmission elements +- Gazebo extensions and many more + +Example: +```xml + + + + +``` + +Or explicitly: +```xml + + + +``` + +#### Version 1.1 + +Version 1.1 extends the URDF specification with the following new features: +- All features from version 1.0 +- **Quaternion orientation**: The `quat_xyzw` attribute can be used in `` elements as an alternative to `rpy` for specifying orientation +- **Capsule geometry**: A new geometry primitive defined by `radius` and `length` attributes + +**Quaternion Example:** +```xml + + + + + + + + + + +``` + +**Note**: You cannot use both `rpy` and `quat_xyzw` in the same `` element. + +**Capsule Geometry Example:** +```xml + + + + + + + + + + + + + + +``` + +The capsule is a cylinder capped with hemispheres at both ends. The `length` attribute specifies the length of the cylindrical portion (not including the hemispherical caps), and `radius` specifies the radius of both the cylinder and the hemispheres. Both attributes must be non-negative finite values. + +#### Compatibility and Parsing Behavior + +| Scenario | Behavior | +|----------|----------| +| **Newer URDF parsed by older library** | Parsing will fail. The library validates the version and rejects URDF files with unsupported versions (e.g., a 1.1 URDF parsed by a library that only supports 1.0). Version-specific features like `quat_xyzw` and `capsule` geometry will be ignored with a warning if the declared version doesn't support them. | +| **Older URDF parsed by newer library** | Fully supported. The newer library is backward compatible and will parse older URDF files correctly. If no version attribute is specified, the parser defaults to version 1.0. | +| **Unsupported version (e.g., 2.0)** | Parsing will fail with an error. Only versions 1.0 and 1.1 are currently supported. | +| **Invalid version format** | Parsing will fail. The version must be in the format `x.y` (e.g., `"1.0"`). Single integers like `"1"` or malformed versions like `"1.0.0"` are rejected. | + +**Version Format Requirements:** +- Must be in the form `major.minor` (e.g., `"1.0"`, `"1.1"`) +- Both major and minor must be non-negative integers +- Single integers (e.g., `"1"`) are not valid +- Trailing characters (e.g., `"1.0~pre6"`) are not allowed + ### Installing from Source with ROS Debians **Warning: this will break ABI compatibility with future /opt/ros updates through the debian package manager. This is a hack, use at your own risk.** From 0a0d9a4d647ea23e1feca7952c9b2b5a1e7980b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 30 Jan 2026 09:39:22 +0100 Subject: [PATCH 3/5] Add tests for the capsule geometry Signed-off-by: Sai Kishor Kothakota --- urdf_parser/test/CMakeLists.txt | 1 + .../urdf_schema_capsule_geometry_test.cpp | 322 ++++++++++++++++++ 2 files changed, 323 insertions(+) create mode 100644 urdf_parser/test/urdf_schema_capsule_geometry_test.cpp diff --git a/urdf_parser/test/CMakeLists.txt b/urdf_parser/test/CMakeLists.txt index e72f6d18..ca6a85e5 100644 --- a/urdf_parser/test/CMakeLists.txt +++ b/urdf_parser/test/CMakeLists.txt @@ -16,6 +16,7 @@ execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results # unit test to fix geometry problems set(tests urdf_double_convert.cpp + urdf_schema_capsule_geometry_test.cpp urdf_schema_quaternion_test.cpp urdf_unit_test.cpp urdf_version_test.cpp diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp new file mode 100644 index 00000000..5de24a25 --- /dev/null +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -0,0 +1,322 @@ +// Copyright 2026 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "urdf_model/link.h" +#include "urdf_parser/urdf_parser.h" + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_version_1_1) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ("capsule_test", urdf->name_); + EXPECT_EQ(1u, urdf->links_.size()); + + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + + // Check visual geometry + ASSERT_EQ(1u, link->visual_array.size()); + ASSERT_NE(nullptr, link->visual_array[0]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->visual_array[0]->geometry->type); + + std::shared_ptr visual_capsule = + std::dynamic_pointer_cast(link->visual_array[0]->geometry); + ASSERT_NE(nullptr, visual_capsule); + EXPECT_DOUBLE_EQ(0.05, visual_capsule->radius); + EXPECT_DOUBLE_EQ(0.5, visual_capsule->length); + + // Check collision geometry + ASSERT_EQ(1u, link->collision_array.size()); + ASSERT_NE(nullptr, link->collision_array[0]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->collision_array[0]->geometry->type); + + std::shared_ptr collision_capsule = + std::dynamic_pointer_cast(link->collision_array[0]->geometry); + ASSERT_NE(nullptr, collision_capsule); + EXPECT_DOUBLE_EQ(0.1, collision_capsule->radius); + EXPECT_DOUBLE_EQ(1.0, collision_capsule->length); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_ignored_version_1_0) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Capsule is not supported in version 1.0 and is ignored with a warning. + // The link is still created, but visual parsing fails. + ASSERT_NE(nullptr, urdf); + EXPECT_EQ("capsule_ignored_test", urdf->name_); + + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + // Visual should be empty since capsule geometry was ignored + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + + std::shared_ptr capsule = + std::dynamic_pointer_cast(link->visual_array[0]->geometry); + ASSERT_NE(nullptr, capsule); + EXPECT_DOUBLE_EQ(0.0, capsule->radius); + EXPECT_DOUBLE_EQ(0.0, capsule->length); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative radius causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative length causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Missing radius causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_length_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Missing length causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_inf_value_fails) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Infinity value causes geometry parsing to fail, visual is empty + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_multiple_capsules_in_same_link) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + + + + + + + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ("multi_capsule_test", urdf->name_); + + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + + // Check multiple visual geometries + ASSERT_EQ(2u, link->visual_array.size()); + + ASSERT_NE(nullptr, link->visual_array[0]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->visual_array[0]->geometry->type); + std::shared_ptr visual_capsule_0 = + std::dynamic_pointer_cast(link->visual_array[0]->geometry); + ASSERT_NE(nullptr, visual_capsule_0); + EXPECT_DOUBLE_EQ(0.05, visual_capsule_0->radius); + EXPECT_DOUBLE_EQ(0.5, visual_capsule_0->length); + + ASSERT_NE(nullptr, link->visual_array[1]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->visual_array[1]->geometry->type); + std::shared_ptr visual_capsule_1 = + std::dynamic_pointer_cast(link->visual_array[1]->geometry); + ASSERT_NE(nullptr, visual_capsule_1); + EXPECT_DOUBLE_EQ(0.1, visual_capsule_1->radius); + EXPECT_DOUBLE_EQ(1.0, visual_capsule_1->length); + + // Check multiple collision geometries + ASSERT_EQ(2u, link->collision_array.size()); + + ASSERT_NE(nullptr, link->collision_array[0]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->collision_array[0]->geometry->type); + std::shared_ptr collision_capsule_0 = + std::dynamic_pointer_cast(link->collision_array[0]->geometry); + ASSERT_NE(nullptr, collision_capsule_0); + EXPECT_DOUBLE_EQ(0.15, collision_capsule_0->radius); + EXPECT_DOUBLE_EQ(1.5, collision_capsule_0->length); + + ASSERT_NE(nullptr, link->collision_array[1]->geometry); + EXPECT_EQ(urdf::Geometry::CAPSULE, link->collision_array[1]->geometry->type); + std::shared_ptr collision_capsule_1 = + std::dynamic_pointer_cast(link->collision_array[1]->geometry); + ASSERT_NE(nullptr, collision_capsule_1); + EXPECT_DOUBLE_EQ(0.2, collision_capsule_1->radius); + EXPECT_DOUBLE_EQ(2.0, collision_capsule_1->length); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // use the environment locale so that the unit test can be repeated with various locales easily + setlocale(LC_ALL, ""); + + return RUN_ALL_TESTS(); +} + From be9fae5edb4c65779e79c57013460355ad67f8d6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Feb 2026 10:05:21 +0100 Subject: [PATCH 4/5] remove invalid number checks for consistency Signed-off-by: Sai Kishor Kothakota --- urdf_parser/src/link.cpp | 6 --- .../urdf_schema_capsule_geometry_test.cpp | 44 ------------------- 2 files changed, 50 deletions(-) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 086783ca..dab48c29 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -246,12 +246,6 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } - if (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length < 0 || c.radius < 0) - { - CONSOLE_BRIDGE_logError("Capsule length and radius must be non-negative finite values"); - return false; - } - return true; } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index 5de24a25..b586a5d0 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -124,50 +124,6 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) EXPECT_DOUBLE_EQ(0.0, capsule->length); } -TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails) -{ - std::string urdf_str = R"urdf( - - - - - - - - - - )urdf"; - - urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - // Negative radius causes geometry parsing to fail, visual is empty - ASSERT_NE(nullptr, urdf); - urdf::LinkConstSharedPtr link = urdf->getLink("link1"); - ASSERT_NE(nullptr, link); - EXPECT_TRUE(link->visual_array.empty()); -} - -TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails) -{ - std::string urdf_str = R"urdf( - - - - - - - - - - )urdf"; - - urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - // Negative length causes geometry parsing to fail, visual is empty - ASSERT_NE(nullptr, urdf); - urdf::LinkConstSharedPtr link = urdf->getLink("link1"); - ASSERT_NE(nullptr, link); - EXPECT_TRUE(link->visual_array.empty()); -} - TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) { std::string urdf_str = R"urdf( From 68235768b01ec54924e3cf7214444812c3d0535d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 3 Feb 2026 09:06:08 -0800 Subject: [PATCH 5/5] Require version 2.1.0 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 32ab5f11..93ca3995 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") find_package(TinyXML2 REQUIRED) -find_package(urdfdom_headers 2.0.2 REQUIRED) +find_package(urdfdom_headers 2.1.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. find_package(console_bridge REQUIRED)