From a60ebef8fe6bb5015960b6850b9adb5d8bafedf8 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume@dexory.com>
Date: Sat, 20 Jan 2024 10:36:38 +0000
Subject: [PATCH 1/2] parse quat_xyzw

---
 src/urdf_parser_py/urdf.py                | 11 ++++++++---
 src/urdf_parser_py/xml_reflection/core.py |  7 ++++++-
 2 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/src/urdf_parser_py/urdf.py b/src/urdf_parser_py/urdf.py
index 41fa2b7..f3155ad 100644
--- a/src/urdf_parser_py/urdf.py
+++ b/src/urdf_parser_py/urdf.py
@@ -9,18 +9,22 @@
 
 xmlr.add_type('element_link', xmlr.SimpleElementType('link', str))
 xmlr.add_type('element_xyz', xmlr.SimpleElementType('xyz', 'vector3'))
+xmlr.add_type('element_quat_xyzw', xmlr.SimpleElementType('quat_xyzw', 'vector4'))
 
 verbose = True
 
 
 class Pose(xmlr.Object):
-    def __init__(self, xyz=None, rpy=None):
+    def __init__(self, xyz=None, rpy=None, quat_xyzw=None):
         self.xyz = xyz
         self.rpy = rpy
+        self.quat_xyzw = quat_xyzw
 
     def check_valid(self):
         assert (self.xyz is None or len(self.xyz) == 3) and \
-            (self.rpy is None or len(self.rpy) == 3)
+            (self.rpy is None or len(self.rpy) == 3) and \
+            (self.quat_xyzw is None or len(self.quat_xyzw) == 4) and \
+            (self.rpy is None or self.quat_xyzw is None)
 
     # Aliases for backwards compatibility
     @property
@@ -38,7 +42,8 @@ def position(self, value): self.xyz = value
 
 xmlr.reflect(Pose, tag='origin', params=[
     xmlr.Attribute('xyz', 'vector3', False, default=[0, 0, 0]),
-    xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0])
+    xmlr.Attribute('rpy', 'vector3', False, default=[0, 0, 0]),
+    xmlr.Attribute('quat_xyzw', 'vector4', False)
 ])
 
 
diff --git a/src/urdf_parser_py/xml_reflection/core.py b/src/urdf_parser_py/xml_reflection/core.py
index b46df35..aede5f7 100644
--- a/src/urdf_parser_py/xml_reflection/core.py
+++ b/src/urdf_parser_py/xml_reflection/core.py
@@ -521,7 +521,12 @@ def get_element_path(element):
         # and find where the problematic parent element.
         for attribute in map(self.attribute_map.get, unset_attributes):
             try:
-                attribute.set_default(obj)
+                if attribute.xml_var == 'rpy':
+                  if 'quat_xyzw' in unset_attributes:
+                    attribute.set_default(obj)
+                else:
+                  attribute.set_default(obj)
+                  
             except ParseError:
                 raise
             except Exception as e:

From 3ea149f662c41b21339564df9fd22759d1a02565 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <guillaume@dexory.com>
Date: Sat, 20 Jan 2024 10:44:17 +0000
Subject: [PATCH 2/2] add test

---
 test/test_urdf.py | 34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/test/test_urdf.py b/test/test_urdf.py
index b78432d..534e2d6 100644
--- a/test/test_urdf.py
+++ b/test/test_urdf.py
@@ -273,6 +273,7 @@ def test_robot_link_defaults(self):
         origin = robot.links[0].inertial.origin
         self.assertEqual(origin.xyz, [0, 0, 0])
         self.assertEqual(origin.rpy, [0, 0, 0])
+        self.assertEqual(origin.quat_xyzw, None)
 
     def test_robot_link_defaults_xyz_set(self):
         xml = '''<?xml version="1.0"?>
@@ -288,7 +289,40 @@ def test_robot_link_defaults_xyz_set(self):
         origin = robot.links[0].inertial.origin
         self.assertEqual(origin.xyz, [1, 2, 3])
         self.assertEqual(origin.rpy, [0, 0, 0])
+        self.assertEqual(origin.quat_xyzw, None)
+        
+    def test_robot_link_defaults_xyz_rpy_set(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="test">
+  <link name="test_link">
+    <inertial>
+      <mass value="10.0"/>
+      <origin xyz="1 2 3" rpy="1 2 3"/>
+    </inertial>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        origin = robot.links[0].inertial.origin
+        self.assertEqual(origin.xyz, [1, 2, 3])
+        self.assertEqual(origin.rpy, [1, 2, 3])
+        self.assertEqual(origin.quat_xyzw, None)
 
+    def test_robot_link_defaults_xyz_quat_xyzw_set(self):
+        xml = '''<?xml version="1.0"?>
+<robot name="test">
+  <link name="test_link">
+    <inertial>
+      <mass value="10.0"/>
+      <origin xyz="1 2 3" quat_xyzw="0.1 0.2 0.3 0.4"/>
+    </inertial>
+  </link>
+</robot>'''
+        robot = self.parse(xml)
+        origin = robot.links[0].inertial.origin
+        self.assertEqual(origin.xyz, [1, 2, 3])
+        self.assertEqual(origin.rpy, None)
+        self.assertEqual(origin.quat_xyzw, [0.1, 0.2, 0.3, 0.4])
+        
     def test_xml_with_UTF8_encoding(self):
         xml = b'''<?xml version="1.0" encoding="UTF-8"?>
 <robot name="test">