diff --git a/README.md b/README.md index 927e7172..6c89bac3 100644 --- a/README.md +++ b/README.md @@ -110,6 +110,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 `NaN` 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 +139,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 `NaN` when omitted | +| `upper` | Optional, defaults to `0.0` when omitted | Optional for non-revolute/non-prismatic joints, defaults to `NaN` 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/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 8580cb15..6bc8c481 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::signaling_NaN(); + } + 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::signaling_NaN(); + } + 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/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 6e8d87e1..aea64dac 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -537,6 +537,373 @@ 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_nan) +{ + std::string joint_str = + "" + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str); + + ASSERT_NE(nullptr, urdf); + EXPECT_TRUE(std::isnan(urdf->joints_["j1"]->limits->lower)); + EXPECT_TRUE(std::isnan(urdf->joints_["j1"]->limits->upper)); + 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 = diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index de8fd4e6..3c8801e0 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -208,10 +208,10 @@ - - - - + + + +