From 82ffa2954c5f70fa980ec13e4c06dc91c5afac1c Mon Sep 17 00:00:00 2001
From: Alex Zastrow <alexzastrow@posteo.de>
Date: Tue, 9 Jan 2024 07:30:50 +0100
Subject: [PATCH] Fix link pose not exported in sdf #338 (1) #339

Exporting pose relative to root link because gazebo 9 ignores relative_to attribute
---
 phobos/blender/io/blender2phobos.py | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/phobos/blender/io/blender2phobos.py b/phobos/blender/io/blender2phobos.py
index 0cc600dd..33926180 100644
--- a/phobos/blender/io/blender2phobos.py
+++ b/phobos/blender/io/blender2phobos.py
@@ -430,11 +430,18 @@ def deriveLink(obj, objectlist=None, logging=True, errors=None):
                     annotations[k1] = {}
                 annotations[k1][k2] = v
 
+    root = obj
+    while sUtils.getEffectiveParent(root) is not None:
+        root = sUtils.getEffectiveParent(root)
+
+    # Exporting pose relative to root link because gazebo 9 ignores relative_to attribute
+
     return representation.Link(
         name=obj.get("link/name", obj.name),  # this is for backwards compatibility normally the linkname should be the object name
         visuals=visuals,
         collisions=collisions,
         inertial=inertial,
+        #origin=deriveObjectPose(obj, effectiveparent=root),
         # [TODO v2.1.0] Add KCCD support
         kccd_hull=None,
         **annotations