diff --git a/.github/workflows/ros2.yaml b/.github/workflows/ros2.yaml index 7a424894..6ad6b2f2 100644 --- a/.github/workflows/ros2.yaml +++ b/.github/workflows/ros2.yaml @@ -15,6 +15,7 @@ jobs: name: 'ROS 2 CI (${{ matrix.build_name }})' runs-on: ubuntu-24.04 strategy: + fail-fast: false matrix: include: - build_name: 'source deps' diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5ae6515e..f3e4aaf6 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package urdfdom ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.0 (2026-04-20) +----------- +* Support for URDF Specification 1.2 + * Extend parsing of acceleration, deceleration and jerk limits from ``limit`` tag (`#212 `_) + * Update default limits for the joint limits and safety limits (`#249 `_) + * Add invalid data checks to the Geometry data (`#242 `_) + * Require urdfdom_headers 3.0.0 (`#257 `_) +* Use URDF_MAJOR_VERSION for SOVERSION (`#248 `_) +* Contributors: Jose Luis Rivero, Sai Kishor Kothakota, Steve Peters + +5.1.2 (2026-04-18) +----------- +* Revert "Extend parsing of acceleration, deceleration and jerk limits from `limit` tag (`#212 `_)" + This was a breaking change that will be released in 6.0.0 +* Contributors: Steve Peters + +5.1.1 (2026-04-15) +------------------ + +* Prevent CI from failing fast to allow all builds to complete (`#254 `_) +* Remove ``urdf_world/types.h`` deprecation (`#251 `_) +* Extend parsing of acceleration, deceleration and jerk limits from ``limit`` tag (`#212 `_) +* ROS 2 CI: build urdfdom_headers from source (`#246 `_) +* Disable system workflow because ``urdfdom_headers`` isn't available on Ubuntu 24.04 (`#240 `_) +* Fix ROS 2 CI workflow by updating Ubuntu version and checkout action (`#239 `_) + +* Contributors: Alejandro Hernández Cordero, Sai Kishor Kothakota, Steve Peters + 5.1.0 (2026-02-06) ------------------ * Support for URDF Specification 1.1 diff --git a/CMakeLists.txt b/CMakeLists.txt index 93ca3995..32c84a1f 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.1.0 REQUIRED) +find_package(urdfdom_headers 3.0.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. find_package(console_bridge REQUIRED) diff --git a/README.md b/README.md index 927e7172..ae4f5a6c 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,16 @@ -urdfdom -=========== +# urdfdom -The URDF (U-Robot Description Format) library provides core data structures and a simple XML parsers for populating the class data structures from an URDF file. +The URDF (U-Robot Description Format) library provides core data structures and simple XML parsers for populating the class data structures from a URDF file. -For now, the details of the URDF specifications reside on http://ros.org/wiki/urdf +Like the accompanying `urdfdom_headers`, this repository is a **custom fork** that includes an implementation of a C++ parser with specialized support for **constraint descriptions** to safely model closed-loop kinematics (e.g., for parallel robots). + +## Constraints Feature + +This fork introduces `constraint` elements representing "cut joints" or "cut links" allowing the description of closed kinematic loops without breaking surrounding packages. +- It parses constraints exactly like the standard URDF `joint` element structure but includes a `child_frame` origin and a `child` axis to effectively describe the two frames of the cut element. +- The constraint structure depends on the custom types (`UNKNOWN`, `REVOLUTE`, `PRISMATIC`, `UNIVERSAL`, `SPHERICAL`, and `LINK`). + +For now, the details of the general URDF specifications reside on http://ros.org/wiki/urdf ### Using with ROS @@ -110,6 +117,13 @@ Version 1.2 extends the URDF specification with extended joint limits: - **Deceleration limit**: The `deceleration` attribute specifies the maximum joint deceleration - **Jerk limit**: The `jerk` attribute specifies the maximum joint jerk (rate of change of acceleration) +Version 1.2 also relaxes some requirements on the existing joint limit attributes: +- `lower` and `upper` are optional for non-`revolute`/non-`prismatic` joints and default to `-infinity` and `infinity` when omitted +- for `revolute` and `prismatic` joints, both `lower` and `upper` must be provided +- `effort` and `velocity` become optional and default to infinity when omitted +- `effort`, `velocity`, `acceleration`, `deceleration`, and `jerk` must all be non-negative when provided +- when both are specified, `upper` must be greater than or equal to `lower` + All three attributes are optional and default to infinity (no limit). If `acceleration` is specified but `deceleration` is not, `deceleration` defaults to the acceleration value. All values must be non-negative. **Extended Joint Limits Example:** @@ -132,6 +146,23 @@ All three attributes are optional and default to infinity (no limit). If `accele | `deceleration` | Maximum joint deceleration (rad/s² or m/s²) | Same as `acceleration`, or infinity if `acceleration` is not set | Must be non-negative | | `jerk` | Maximum joint jerk (rad/s³ or m/s³) | Infinity | Must be non-negative | +#### Joint Limit Semantics By Version + +The parser keeps legacy behavior for older URDF versions and only applies the relaxed defaults starting in version 1.2. + +| Attribute | Version 1.0 / 1.1 | Version 1.2 | +|-----------|-------------------|-------------| +| `lower` | Optional, defaults to `0.0` when omitted | Optional for non-revolute/non-prismatic joints, defaults to `-infinity` when omitted | +| `upper` | Optional, defaults to `0.0` when omitted | Optional for non-revolute/non-prismatic joints, defaults to `infinity` when omitted | +| `effort` | Required, parsing fails if omitted | Optional, defaults to `infinity` when omitted | +| `velocity` | Required, parsing fails if omitted | Optional, defaults to `infinity` when omitted | +| `acceleration` | Ignored with warning if provided | Optional, defaults to `infinity` when omitted | +| `deceleration` | Ignored with warning if provided | Optional, defaults to `acceleration` if set, otherwise infinity | +| `jerk` | Ignored with warning if provided | Optional, defaults to `infinity` when omitted | + +For all supported versions, invalid numeric values still fail parsing. In version 1.2, negative values for `effort`, `velocity`, `acceleration`, `deceleration`, and `jerk` are rejected. +In version 1.2, `revolute` and `prismatic` joints additionally require both `lower` and `upper`, and `upper` cannot be smaller than `lower`. + #### Compatibility and Parsing Behavior | Scenario | Behavior | diff --git a/package.xml b/package.xml index dbdcae1c..f18a1181 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ urdfdom - 5.1.0 + 6.0.0 A library to access URDFs using the DOM model. Chris Lalancette diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index a0cef4d7..073fbc78 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -22,7 +22,8 @@ macro(add_urdfdom_library) endif() set_target_properties(${add_urdfdom_library_LIBNAME} PROPERTIES DEFINE_SYMBOL URDFDOM_EXPORTS - SOVERSION ${URDF_MAJOR_MINOR_VERSION}) + SOVERSION ${URDF_MAJOR_VERSION} + VERSION ${URDF_VERSION}) endmacro() if(TARGET console_bridge::console_bridge) @@ -45,6 +46,7 @@ add_urdfdom_library( src/model.cpp src/link.cpp src/joint.cpp + src/constraint.cpp src/world.cpp) add_urdfdom_library( @@ -54,6 +56,7 @@ add_urdfdom_library( src/pose.cpp src/model.cpp src/link.cpp + src/constraint.cpp src/joint.cpp) add_urdfdom_library( diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 0e725b44..c8f20c3e 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -43,10 +43,11 @@ #include #include +#include #include #include #include -#include +#include #include "exportdecl.h" diff --git a/urdf_parser/src/constraint.cpp b/urdf_parser/src/constraint.cpp new file mode 100644 index 00000000..9583bd13 --- /dev/null +++ b/urdf_parser/src/constraint.cpp @@ -0,0 +1,280 @@ +/********************************************************************* +* Software Ligcense Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Fabian Finkbeiner */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "./pose.hpp" + +namespace urdf{ + + +bool parseConstraint(Constraint &constraint, tinyxml2::XMLElement* config, + const urdf_export_helpers::URDFVersion version) +{ + constraint.clear(); + + // Get Constraint Name + const char *name = config->Attribute("name"); + if (!name) + { + CONSOLE_BRIDGE_logError("unnamed constraint found"); + return false; + } + constraint.name = name; + + // Get transform from Parent Link to Constraint Parent Frame + tinyxml2::XMLElement *parent_origin_xml = config->FirstChildElement("parent_origin"); + if (!parent_origin_xml) + { + CONSOLE_BRIDGE_logDebug("urdfdom: Constraint [%s] missing parent_origin tag under parent describing transform from Parent Link to Constraint Parent Frame, (using Identity transform).", constraint.name.c_str()); + constraint.parent_to_constraint_parent_transform.clear(); + } + else + { + if (!parsePoseInternal(constraint.parent_to_constraint_parent_transform, parent_origin_xml, version)) + { + constraint.parent_to_constraint_parent_transform.clear(); + CONSOLE_BRIDGE_logError("Malformed parent origin element for constraint [%s]", constraint.name.c_str()); + return false; + } + } + + + // Get transform from Child Link to Constraint Child Frame + tinyxml2::XMLElement *child_origin_xml = config->FirstChildElement("child_origin"); + if (!child_origin_xml) + { + CONSOLE_BRIDGE_logDebug("urdfdom: Constraint [%s] missing child_origin tag under parent describing transform from Child Link to Constraint Child Frame, (using Identity transform).", constraint.name.c_str()); + constraint.child_to_constraint_child_transform.clear(); + } + else + { + if (!parsePoseInternal(constraint.child_to_constraint_child_transform, child_origin_xml, version)) + { + constraint.child_to_constraint_child_transform.clear(); + CONSOLE_BRIDGE_logError("Malformed child origin element for constraint [%s]", constraint.name.c_str()); + return false; + } + } + + // Get Parent Link + tinyxml2::XMLElement *parent_xml = config->FirstChildElement("parent"); + if (parent_xml) + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + CONSOLE_BRIDGE_logInform("no parent link name specified for Constraint parent_link [%s]", constraint.name.c_str()); + } + else + { + constraint.parent_link_name = std::string(pname); + } + } + + // Get Child Link + tinyxml2::XMLElement *child_xml = config->FirstChildElement("child"); + if (child_xml) + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + CONSOLE_BRIDGE_logInform("no child link name specified for Constraint child_link [%s].", constraint.name.c_str()); + } + else + { + constraint.child_link_name = std::string(pname); + } + } + + // Get Constraint type + const char* type_char = config->Attribute("type"); + if (!type_char) + { + CONSOLE_BRIDGE_logError("constraint [%s] has no type, check to see if it's a reference.", constraint.name.c_str()); + return false; + } + + std::string type_str = type_char; + if (type_str == "revolute" || type_str == "revolute_joint") + constraint.type = Constraint::REVOLUTE; + else if (type_str == "prismatic" || type_str == "prismatic_joint") + constraint.type = Constraint::PRISMATIC; + else if (type_str == "universal" || type_str == "universal_joint") + constraint.type = Constraint::UNIVERSAL; + else if (type_str == "spherical" || type_str == "spherical_joint") + constraint.type = Constraint::SPHERICAL; + else if (type_str == "link") + constraint.type = Constraint::LINK; + else + { + CONSOLE_BRIDGE_logError("Constraint [%s] has no known type [%s]", constraint.name.c_str(), type_str.c_str()); + return false; + } + + + // Get Constraint parent Axis + if (constraint.type != Constraint::LINK && constraint.type != Constraint::SPHERICAL) + { + // axis + tinyxml2::XMLElement *parent_axis_xml = config->FirstChildElement("parent_axis"); + if (!parent_axis_xml){ + CONSOLE_BRIDGE_logDebug("urdfdom: no parent_axis element for Constraint [%s], defaulting to (1,0,0) axis", constraint.name.c_str()); + constraint.parent_axis = Vector3(1.0, 0.0, 0.0); + } + else{ + if (parent_axis_xml->Attribute("xyz")){ + try { + constraint.parent_axis.init(parent_axis_xml->Attribute("xyz")); + } + catch (ParseError &e) { + constraint.parent_axis.clear(); + CONSOLE_BRIDGE_logError("Malformed axis element for constraint [%s]: %s", constraint.name.c_str(), e.what()); + return false; + } + } + } + } + + // Get Constraint child Axis + if (constraint.type == Constraint::UNIVERSAL) + { + // axis + tinyxml2::XMLElement *child_axis_xml = config->FirstChildElement("child_axis"); + if (!child_axis_xml){ + CONSOLE_BRIDGE_logDebug("urdfdom: no child_axis element for Constraint [%s], defaulting to (1,0,0) axis", constraint.name.c_str()); + constraint.child_axis = Vector3(1.0, 0.0, 0.0); + } + else{ + if (child_axis_xml->Attribute("xyz")){ + try { + constraint.child_axis.init(child_axis_xml->Attribute("xyz")); + } + catch (ParseError &e) { + constraint.child_axis.clear(); + CONSOLE_BRIDGE_logError("Malformed axis element for constraint [%s]: %s", constraint.name.c_str(), e.what()); + return false; + } + } + } + } + return true; +} + + +/* exports */ +bool exportParentPose(Pose &pose, tinyxml2::XMLElement* xml) +{ + tinyxml2::XMLElement* origin = xml->GetDocument()->NewElement("parent_origin"); + std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position); + std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation); + origin->SetAttribute("xyz", pose_xyz_str.c_str()); + origin->SetAttribute("rpy", pose_rpy_str.c_str()); + xml->LinkEndChild(origin); + return true; +} + +bool exportChildPose(Pose &pose, tinyxml2::XMLElement* xml) +{ + tinyxml2::XMLElement* origin = xml->GetDocument()->NewElement("child_origin"); + std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position); + std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation); + origin->SetAttribute("xyz", pose_xyz_str.c_str()); + origin->SetAttribute("rpy", pose_rpy_str.c_str()); + xml->LinkEndChild(origin); + return true; +} + +bool exportConstraint(Constraint &constraint, tinyxml2::XMLElement* xml) +{ + tinyxml2::XMLElement * constraint_xml = xml->GetDocument()->NewElement("constraint"); + constraint_xml->SetAttribute("name", constraint.name.c_str()); + if (constraint.type == urdf::Constraint::REVOLUTE) + constraint_xml->SetAttribute("type", "revolute_joint"); + else if (constraint.type == urdf::Constraint::PRISMATIC) + constraint_xml->SetAttribute("type", "prismatic_joint"); + else if (constraint.type == urdf::Constraint::UNIVERSAL) + constraint_xml->SetAttribute("type", "universal_joint"); + else if (constraint.type == urdf::Constraint::SPHERICAL) + constraint_xml->SetAttribute("type", "spherical_joint"); + else if (constraint.type == urdf::Constraint::LINK) + constraint_xml->SetAttribute("type", "link"); + else + CONSOLE_BRIDGE_logError("ERROR: Constraint [%s] type [%d] is not a defined type.\n",constraint.name.c_str(), constraint.type); + + // origins + exportParentPose(constraint.parent_to_constraint_parent_transform, constraint_xml); + exportChildPose(constraint.child_to_constraint_child_transform, constraint_xml); + + // axes + if(constraint.type != Constraint::LINK && constraint.type!=Constraint::SPHERICAL) + { + tinyxml2::XMLElement * parent_axis_xml = constraint_xml->GetDocument()->NewElement("parent_axis"); + parent_axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(constraint.parent_axis).c_str()); + constraint_xml->LinkEndChild(parent_axis_xml); + } + + if (constraint.type==Constraint::UNIVERSAL) + { + tinyxml2::XMLElement * child_axis_xml = constraint_xml->GetDocument()->NewElement("child_axis"); + child_axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(constraint.child_axis).c_str()); + constraint_xml->LinkEndChild(child_axis_xml); + } + // parent + tinyxml2::XMLElement * parent_xml = constraint_xml->GetDocument()->NewElement("parent"); + parent_xml->SetAttribute("link", constraint.parent_link_name.c_str()); + constraint_xml->LinkEndChild(parent_xml); + + // child + tinyxml2::XMLElement * child_xml = constraint_xml->GetDocument()->NewElement("child"); + child_xml->SetAttribute("link", constraint.child_link_name.c_str()); + constraint_xml->LinkEndChild(child_xml); + + xml->LinkEndChild(constraint_xml); + return true; +} + + +} + + diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 8580cb15..69d26f16 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -96,15 +96,25 @@ bool parseJointDynamics(JointDynamics &jd, tinyxml2::XMLElement* config) bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config, const urdf_export_helpers::URDFVersion version, - const std::string& joint_name) + const std::string& joint_name, int joint_type) { jl.clear(); // Get lower joint limit + bool lower_pos_limit = true; + bool upper_pos_limit = true; const char* lower_str = config->Attribute("lower"); if (lower_str == NULL){ - CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: joint [%s] has no lower, defaults to 0", joint_name.c_str()); - jl.lower = 0; + if (version.less_than(1, 2)) + { + jl.lower = 0.0; + } + else + { + jl.lower = -std::numeric_limits::infinity(); + } + CONSOLE_BRIDGE_logInform("urdfdom.joint_limit: joint [%s] has no lower, defaults to %f", joint_name.c_str(), jl.lower); + lower_pos_limit = false; } else { @@ -119,8 +129,16 @@ bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config, // Get upper joint limit const char* upper_str = config->Attribute("upper"); if (upper_str == NULL){ - CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: joint [%s] has no upper, defaults to 0", joint_name.c_str()); - jl.upper = 0; + if (version.less_than(1, 2)) + { + jl.upper = 0.0; + } + else + { + jl.upper = std::numeric_limits::infinity(); + } + CONSOLE_BRIDGE_logInform("urdfdom.joint_limit: joint [%s] has no upper position limit, defaults to %f", joint_name.c_str(), jl.upper); + upper_pos_limit = false; } else { @@ -132,11 +150,37 @@ bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config, } } + if (version.at_least(1, 2)) + { + if(!lower_pos_limit || !upper_pos_limit) + { + if (joint_type == Joint::REVOLUTE || joint_type == Joint::PRISMATIC) + { + CONSOLE_BRIDGE_logError("joint [%s]: revolute and prismatic joints must have both lower and upper position limits specified in URDF version 1.2 or later", joint_name.c_str()); + return false; + } + } + + if (lower_pos_limit && upper_pos_limit && jl.upper < jl.lower) + { + CONSOLE_BRIDGE_logError("joint [%s]: upper position limit (%f) cannot be smaller than lower position limit (%f)", joint_name.c_str(), jl.upper, jl.lower); + return false; + } + } + // Get joint effort limit const char* effort_str = config->Attribute("effort"); if (effort_str == NULL){ - CONSOLE_BRIDGE_logError("joint [%s]: limit has no effort", joint_name.c_str()); - return false; + if (version.less_than(1, 2)) + { + CONSOLE_BRIDGE_logError("joint [%s]: limit has no effort", joint_name.c_str()); + return false; + } + else + { + jl.effort = std::numeric_limits::infinity(); + CONSOLE_BRIDGE_logInform("urdfdom.joint_limit: joint [%s] has no effort limit, defaults to %f", joint_name.c_str(), jl.effort); + } } else { @@ -156,8 +200,16 @@ bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config, // Get joint velocity limit const char* velocity_str = config->Attribute("velocity"); if (velocity_str == NULL){ - CONSOLE_BRIDGE_logError("joint [%s]: limit has no velocity", joint_name.c_str()); - return false; + if (version.less_than(1, 2)) + { + CONSOLE_BRIDGE_logError("joint [%s]: limit has no velocity", joint_name.c_str()); + return false; + } + else + { + jl.velocity = std::numeric_limits::infinity(); + CONSOLE_BRIDGE_logInform("urdfdom.joint_limit: joint [%s] has no velocity limit, defaults to %f", joint_name.c_str(), jl.velocity); + } } else { @@ -543,7 +595,7 @@ bool parseJoint(Joint &joint, tinyxml2::XMLElement* config, if (limit_xml) { joint.limits.reset(new JointLimits()); - if (!parseJointLimits(*joint.limits, limit_xml, version, joint.name)) + if (!parseJointLimits(*joint.limits, limit_xml, version, joint.name, joint.type)) { CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str()); joint.limits.reset(); diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index e7cbdf18..48d32f9a 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -107,7 +107,8 @@ bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_n } -bool parseSphere(Sphere &s, tinyxml2::XMLElement *c) +bool parseSphere(Sphere &s, tinyxml2::XMLElement *c, + const urdf_export_helpers::URDFVersion version) { s.clear(); @@ -127,10 +128,17 @@ bool parseSphere(Sphere &s, tinyxml2::XMLElement *c) return false; } + if (version.at_least(1, 2) && (!std::isfinite(s.radius) || s.radius <= 0)) + { + CONSOLE_BRIDGE_logError("Sphere radius must be a positive finite value"); + return false; + } + return true; } -bool parseBox(Box &b, tinyxml2::XMLElement *c) +bool parseBox(Box &b, tinyxml2::XMLElement *c, + const urdf_export_helpers::URDFVersion version) { b.clear(); @@ -150,10 +158,20 @@ bool parseBox(Box &b, tinyxml2::XMLElement *c) CONSOLE_BRIDGE_logError("%s", e.what()); return false; } + + const bool are_dim_finite = std::isfinite(b.dim.x) && std::isfinite(b.dim.y) && std::isfinite(b.dim.z); + const bool are_dim_positive = b.dim.x > 0 && b.dim.y > 0 && b.dim.z > 0; + if (version.at_least(1, 2) && (!are_dim_finite || !are_dim_positive)) + { + CONSOLE_BRIDGE_logError("Box size must be positive finite values"); + return false; + } + return true; } -bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) +bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c, + const urdf_export_helpers::URDFVersion version) { y.clear(); @@ -183,6 +201,13 @@ bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) return false; } + if (version.at_least(1, 2) && + (!std::isfinite(y.length) || !std::isfinite(y.radius) || y.length <= 0 || y.radius <= 0)) + { + CONSOLE_BRIDGE_logError("Cylinder length and radius must be positive finite values"); + return false; + } + return true; } @@ -216,7 +241,8 @@ bool parseMesh(Mesh &m, tinyxml2::XMLElement *c) return true; } -bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) +bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem, + const urdf_export_helpers::URDFVersion version) { c.clear(); @@ -246,6 +272,13 @@ bool parseCapsule(Capsule &c, tinyxml2::XMLElement *elem) return false; } + if (version.at_least(1, 2) && + (!std::isfinite(c.length) || !std::isfinite(c.radius) || c.length <= 0 || c.radius <= 0)) + { + CONSOLE_BRIDGE_logError("Capsule length and radius must be positive finite values"); + return false; + } + return true; } @@ -267,21 +300,21 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g, { Sphere *s = new Sphere(); geom.reset(s); - if (parseSphere(*s, shape)) + if (parseSphere(*s, shape, version)) return geom; } else if (type_name == "box") { Box *b = new Box(); geom.reset(b); - if (parseBox(*b, shape)) + if (parseBox(*b, shape, version)) return geom; } else if (type_name == "cylinder") { Cylinder *c = new Cylinder(); geom.reset(c); - if (parseCylinder(*c, shape)) + if (parseCylinder(*c, shape, version)) return geom; } else if (type_name == "mesh") @@ -300,7 +333,7 @@ GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g, else { Capsule *c = new Capsule(); geom.reset(c); - if (parseCapsule(*c, shape)) + if (parseCapsule(*c, shape, version)) return geom; } } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 2998827f..7aa6d483 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -49,6 +49,8 @@ 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); +bool parseConstraint(Constraint &constraint, tinyxml2::XMLElement *config, + const urdf_export_helpers::URDFVersion version); ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { @@ -246,6 +248,34 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } } + // Get all Constraint elements + for (tinyxml2::XMLElement* constraint_xml = robot_xml->FirstChildElement("constraint"); constraint_xml; constraint_xml = constraint_xml->NextSiblingElement("constraint")) + { + ConstraintSharedPtr constraint; + constraint.reset(new Constraint); + + if (parseConstraint(*constraint, constraint_xml, version)) + { + if (model->getConstraint(constraint->name)) + { + CONSOLE_BRIDGE_logError("constraint '%s' is not unique.", constraint->name.c_str()); + model.reset(); + return model; + } + else + { + model->constraints_.insert(make_pair(constraint->name,constraint)); + CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new constraint '%s'", constraint->name.c_str()); + } + } + else + { + CONSOLE_BRIDGE_logError("constraint xml is not initialized correctly"); + model.reset(); + return model; + } + } + // every link has children links and joints, but no parents, so we create a // local convenience data structure for keeping child->parent relations @@ -282,6 +312,9 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) bool exportMaterial(Material &material, tinyxml2::XMLElement *config); bool exportLink(Link &link, tinyxml2::XMLElement *config); bool exportJoint(Joint &joint, tinyxml2::XMLElement *config); +bool exportConstraint(Constraint &constraint, tinyxml2::XMLElement *config); + + tinyxml2::XMLDocument* exportURDFInternal(const ModelInterface &model) { @@ -310,6 +343,12 @@ tinyxml2::XMLDocument* exportURDFInternal(const ModelInterface &model) exportJoint(*(j->second), robot); } + for (std::map::const_iterator c=model.constraints_.begin(); c!=model.constraints_.end(); ++c) + { + CONSOLE_BRIDGE_logDebug("urdfdom: exporting constraint [%s]\n",c->second->name.c_str()); + exportConstraint(*(c->second), robot); + } + return doc; } } diff --git a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp index b586a5d0..c37fb2c2 100644 --- a/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp +++ b/urdf_parser/test/urdf_schema_capsule_geometry_test.cpp @@ -97,7 +97,29 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_ignored_version_1_0) EXPECT_TRUE(link->visual_array.empty()); } -TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) +TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Invalid radius/length causes geometry parsing to fail for version >= 1.2 + 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_zero_values_allowed_pre_v1_2) { std::string urdf_str = R"urdf( @@ -112,16 +134,99 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_zero_values) )urdf"; urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Zero radius/length is allowed for version < 1.2 + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_radius_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative radius causes geometry parsing to fail for version >= 1.2 ASSERT_NE(nullptr, urdf); urdf::LinkConstSharedPtr link = urdf->getLink("link1"); ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} - 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_allowed_pre_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative radius is allowed for version < 1.2 + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, parse_capsule_geometry_negative_length_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative length causes geometry parsing to fail for version >= 1.2 + 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_allowed_pre_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + // Negative length is allowed for version < 1.2 + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_FALSE(link->visual_array.empty()); } TEST(URDF_UNIT_TEST, parse_capsule_geometry_missing_radius_fails) @@ -183,7 +288,7 @@ TEST(URDF_UNIT_TEST, parse_capsule_geometry_inf_value_fails) )urdf"; urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); - // Infinity value causes geometry parsing to fail, visual is empty + // "inf" is not parseable by strToDouble, so geometry parsing always fails ASSERT_NE(nullptr, urdf); urdf::LinkConstSharedPtr link = urdf->getLink("link1"); ASSERT_NE(nullptr, link); diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 6e8d87e1..75be31cf 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -537,6 +537,375 @@ TEST(URDF_UNIT_TEST, parse_joint_version_1_2_invalid_acceleration_fails) EXPECT_EQ(nullptr, urdf); } +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_without_lower_upper_sets_infinity) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_TRUE(std::isinf(urdf->joints_["j1"]->limits->lower)); + EXPECT_LT(urdf->joints_["j1"]->limits->lower, 0.0); + EXPECT_TRUE(std::isinf(urdf->joints_["j1"]->limits->upper)); + EXPECT_GT(urdf->joints_["j1"]->limits->upper, 0.0); + EXPECT_EQ(99.0, urdf->joints_["j1"]->limits->effort); + EXPECT_EQ(23.0, urdf->joints_["j1"]->limits->velocity); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_without_effort_velocity_sets_infinity) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(-1.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(1.0, urdf->joints_["j1"]->limits->upper); + EXPECT_TRUE(std::isinf(urdf->joints_["j1"]->limits->effort)); + EXPECT_TRUE(std::isinf(urdf->joints_["j1"]->limits->velocity)); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_revolute_without_upper_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_prismatic_without_lower_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_upper_smaller_than_lower_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_negative_effort_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_2_negative_velocity_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_1_without_effort_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_1_without_velocity_fails) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_0_without_lower_upper_defaults_zero) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_1_without_lower_upper_defaults_zero) +{ + // Version 1.1 omitting lower/upper should also default to 0.0 (same as v1.0) + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_1_only_lower_omits_upper_defaults_zero) +{ + // Providing lower but omitting upper: upper should default to 0.0 in v1.1 + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(-1.5, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_1_only_upper_omits_lower_defaults_zero) +{ + // Providing upper but omitting lower: lower should default to 0.0 in v1.1 + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(2.5, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_0_only_lower_omits_upper_defaults_zero) +{ + // Providing lower but omitting upper: upper should default to 0.0 in v1.0 + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(-1.5, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_version_1_0_only_upper_omits_lower_defaults_zero) +{ + // Providing upper but omitting lower: lower should default to 0.0 in v1.0 + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(0.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(2.5, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_upper_less_than_lower_succeeds_v1_0) +{ + // upper < lower is only rejected from v1.2 onwards; v1.0 allows it + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(1.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(0.5, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_upper_less_than_lower_succeeds_v1_1) +{ + // upper < lower is only rejected from v1.2 onwards; v1.1 allows it + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(2.0, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(1.0, urdf->joints_["j1"]->limits->upper); +} + +TEST(URDF_UNIT_TEST, parse_joint_upper_less_than_lower_fails_v1_2) +{ + // upper < lower must be rejected in v1.2 as well + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + EXPECT_EQ(nullptr, urdf); +} + +TEST(URDF_UNIT_TEST, parse_joint_upper_equal_lower_succeeds) +{ + // upper == lower is valid (zero-range joint, e.g. a fixed position) + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + ASSERT_NE(nullptr, urdf); + EXPECT_EQ(1.5, urdf->joints_["j1"]->limits->lower); + EXPECT_EQ(1.5, urdf->joints_["j1"]->limits->upper); +} + TEST(URDF_UNIT_TEST, parse_link_doubles) { std::string joint_str = @@ -666,6 +1035,298 @@ TEST(URDF_UNIT_TEST, parse_color_doubles) EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz); } +TEST(URDF_UNIT_TEST, negative_sphere_radius_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Invalid radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_sphere_radius_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Invalid radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_box_size_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative size causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_box_size_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Zero size causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_radius_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_length_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Negative length causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_cylinder_radius_fails_v1_2) +{ + std::string urdf_str = R"urdf( + + + + + + + + + + )urdf"; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str); + + // Zero radius causes geometry parsing to fail + ASSERT_NE(nullptr, urdf); + urdf::LinkConstSharedPtr link = urdf->getLink("link1"); + ASSERT_NE(nullptr, link); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_cylinder_length_fails_v1_2) +{ + 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); + EXPECT_TRUE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_sphere_radius_allowed_pre_v1_2) +{ + 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); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, zero_sphere_radius_allowed_pre_v1_2) +{ + 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); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_box_size_allowed_pre_v1_2) +{ + 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); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_radius_allowed_pre_v1_2) +{ + 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); + EXPECT_FALSE(link->visual_array.empty()); +} + +TEST(URDF_UNIT_TEST, negative_cylinder_length_allowed_pre_v1_2) +{ + 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); + EXPECT_FALSE(link->visual_array.empty()); +} int main(int argc, char **argv) { diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index de8fd4e6..c7754b10 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -24,6 +24,17 @@ + + + + + + + + + + + @@ -208,10 +219,10 @@ - - - - + + + + @@ -377,6 +388,26 @@ + + + + + + + + + + + + + + @@ -385,6 +416,8 @@ type="joint" minOccurs="0" maxOccurs="unbounded" /> +