diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 4269419a..2579a4bb 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -140,6 +140,11 @@ bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config) { try { jl.effort = strToDouble(effort_str); + if (jl.effort < 0.0) + { + CONSOLE_BRIDGE_logError("effort value (%s) is negative", effort_str); + return false; + } } catch(std::runtime_error &) { CONSOLE_BRIDGE_logError("effort value (%s) is not a valid float", effort_str); return false; @@ -156,12 +161,80 @@ bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config) { try { jl.velocity = strToDouble(velocity_str); + if (jl.velocity < 0.0) + { + CONSOLE_BRIDGE_logError("velocity value (%s) is negative", velocity_str); + return false; + } } catch(std::runtime_error &) { CONSOLE_BRIDGE_logError("velocity value (%s) is not a valid float", velocity_str); return false; } } + // Get joint acceleration limit + const char* acceleration_str = config->Attribute("acceleration"); + if (acceleration_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no acceleration, using default value"); + jl.acceleration = std::numeric_limits::infinity(); + } + else + { + try { + jl.acceleration = strToDouble(acceleration_str); + if (jl.acceleration < 0.0) + { + CONSOLE_BRIDGE_logError("acceleration value (%s) is negative", velocity_str); + return false; + } + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("acceleration value (%s) is not a valid float", acceleration_str); + return false; + } + } + + // Get joint deceleration limit + const char* deceleration_str = config->Attribute("deceleration"); + if (deceleration_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no deceleration, using acceleration limit"); + jl.deceleration = jl.acceleration; + } + else + { + try { + jl.deceleration = strToDouble(deceleration_str); + if (jl.deceleration < 0.0) + { + CONSOLE_BRIDGE_logError("deceleration value (%s) is negative", deceleration_str); + return false; + } + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("deceleration value (%s) is not a valid float", deceleration_str); + return false; + } + } + + // Get joint jerk limit + const char* jerk_str = config->Attribute("jerk"); + if (jerk_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no jerk, using default value"); + jl.jerk = std::numeric_limits::infinity(); + } + else + { + try { + jl.jerk = strToDouble(jerk_str); + if(jl.jerk < 0.0) + { + CONSOLE_BRIDGE_logError("jerk value (%s) is negative", jerk_str); + return false; + } + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("jerk value (%s) is not a valid float", jerk_str); + return false; + } + } + return true; } diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 8349b1de..2388bb1d 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -178,7 +178,7 @@ TEST(URDF_UNIT_TEST, parse_joint_doubles) " " " " " " - " " + " " " " " " " " @@ -204,6 +204,9 @@ TEST(URDF_UNIT_TEST, parse_joint_doubles) EXPECT_EQ(22.999, urdf->joints_["j1"]->limits->upper); EXPECT_EQ(99.0, urdf->joints_["j1"]->limits->effort); EXPECT_EQ(23.0, urdf->joints_["j1"]->limits->velocity); + EXPECT_EQ(10.0, urdf->joints_["j1"]->limits->acceleration); + EXPECT_EQ(5.0, urdf->joints_["j1"]->limits->deceleration); + EXPECT_EQ(200.0, urdf->joints_["j1"]->limits->jerk); EXPECT_EQ(8.765, urdf->joints_["j1"]->safety->soft_lower_limit); EXPECT_EQ(9.003, urdf->joints_["j1"]->safety->soft_upper_limit); diff --git a/xsd/urdf.xsd b/xsd/urdf.xsd index ccebfb26..b8ac89bb 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -197,6 +197,9 @@ + + +