diff --git a/CMakeLists.txt b/CMakeLists.txt index b5330130..a9676cad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required( VERSION 3.5 FATAL_ERROR ) project (urdfdom CXX C) set (URDF_MAJOR_VERSION 4) -set (URDF_MINOR_VERSION 0) +set (URDF_MINOR_VERSION 1) set (URDF_PATCH_VERSION 0) set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION}) diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index ce6c92b2..f8cbbfb9 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 ccebfb26..853fed5f 100644 --- a/xsd/urdf.xsd +++ b/xsd/urdf.xsd @@ -17,6 +17,7 @@ +