Fix link pose not exported in sdf dfki-ric#338 (1) dfki-ric#339

Exporting pose relative to root link because gazebo 9 ignores relative_to attribute

Author: AlpenAalAlex

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