From 014db9807aebce6741316d53b9bc72089c192870 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume@dexory.com>
Date: Sat, 20 Jan 2024 10:56:38 +0000
Subject: [PATCH] quaternions in urdf (PR 51 new attempt) + bump version

https://github.com/ros/urdfdom_headers/pull/51
Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
---
 CMakeLists.txt            |  4 ++--
 include/urdf_model/pose.h | 49 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6b7de60..f434efa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,8 +13,8 @@ if(APPEND_PROJECT_NAME_TO_INCLUDEDIR)
 endif()
 
 set (URDF_MAJOR_VERSION 1)
-set (URDF_MINOR_VERSION 1)
-set (URDF_PATCH_VERSION 1)
+set (URDF_MINOR_VERSION 2)
+set (URDF_PATCH_VERSION 0)
 
 set (URDF_VERSION ${URDF_MAJOR_VERSION}.${URDF_MINOR_VERSION}.${URDF_PATCH_VERSION})
 
diff --git a/include/urdf_model/pose.h b/include/urdf_model/pose.h
index 13e47b2..e4677f2 100644
--- a/include/urdf_model/pose.h
+++ b/include/urdf_model/pose.h
@@ -88,6 +88,43 @@ class Vector3
   };
 };
 
+class Vector4
+{
+  public:
+  Vector4(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
+  Vector4() {this->clear();};
+  double x;
+  double y;
+  double z;
+  double w;
+  
+  void clear() {this->x=this->y=this->z=this->w=0.0;};
+  void init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<double> xyzw;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i){
+      if (pieces[i] != ""){
+        try {
+          xyzw.push_back(strToDouble(pieces[i].c_str()));
+        } catch(std::runtime_error &) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+    
+    if (xyzw.size() != 4)
+      throw ParseError("Parser found " + std::to_string(xyzw.size())  + " elements but 4 expected while parsing vector [" + vector_str + "]");
+    
+    this->x = xyzw[0];
+    this->y = xyzw[1];
+    this->z = xyzw[2];
+    this->w = xyzw[3];
+  }
+};
+
 class Rotation
 {
 public:
@@ -163,6 +200,18 @@ class Rotation
     rpy.init(rotation_str);
     setFromRPY(rpy.x, rpy.y, rpy.z);
   }
+  
+  void initQuaternion(const std::string &rotation_str)
+  {
+    this->clear();
+    Vector4 xyzw_quat;
+    xyzw_quat.init(rotation_str);
+    this->x = xyzw_quat.x;
+    this->y = xyzw_quat.y;
+    this->z = xyzw_quat.z;
+    this->w = xyzw_quat.w;
+    this->normalize();
+  }
 
   void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }