Fix #339

#339
dfki-ric · Oct 26, 2023 · ea4b644 · ea4b644
1 parent b562322
commit ea4b644
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 11 deletions.
diff --git a/phobos/data/xml_formats.json b/phobos/data/xml_formats.json
@@ -653,7 +653,7 @@
       "children": {
         "pose": {
           "classname": "Pose",
-          "varname": "origin"
+          "varname": "secure_origin"
         },
         "inertial": {
           "classname": "Inertial",

diff --git a/phobos/io/representation.py b/phobos/io/representation.py
@@ -1577,6 +1577,16 @@ def __init__(self, name=None, visuals=None, inertial=None, collisions=None, orig
                     geo.name += str(i)
         self.excludes += ["inertial"]
 
+    @property
+    def secure_origin(self):
+        if self.origin is None:
+            return self.joint_relative_origin
+        return self.origin
+
+    @secure_origin.setter
+    def secure_origin(self, value):
+        self.origin = value
+
     def remove_aggregate(self, elem):
         if isinstance(elem, Visual):
             self.visuals.remove(elem)
@@ -1606,17 +1616,20 @@ def materials(self, value):
     @property
     def joint_relative_origin(self):
         assert self._related_robot_instance is not None
-        assert self.origin is not None
-        out = self.origin
-        if self.origin.relative_to == self._related_robot_instance.get_parent(self):
-            return out
+        if self.origin is not None:
+            out = self.origin
+            if self.origin.relative_to == self._related_robot_instance.get_parent(self):
+                return out
+            else:
+                assert self.origin.relative_to is not None
+                r2x = self._related_robot_instance.get_transformation
+                return Pose.from_matrix(
+                    inv(r2x(self)).dot(r2x(self.origin.relative_to).dot(self.origin.to_matrix())),
+                    relative_to=self._related_robot_instance.get_parent(self)
+                )
         else:
-            assert self.origin.relative_to is not None
-            r2x = self._related_robot_instance.get_transformation
-            return Pose.from_matrix(
-                inv(r2x(self)).dot(r2x(self.origin.relative_to).dot(self.origin.to_matrix())),
-                relative_to=self._related_robot_instance.get_parent(self)
-            )
+            return Pose(relative_to=self._related_robot_instance.get_parent(self))
+
 
     @joint_relative_origin.setter
     def joint_relative_origin(self, value):