[MultiverseAction] added is_inverse to indicate which was the origina…

…l parent of the attachment.

src/pycram/world_concepts/constraints.py

@@ -202,9 +202,10 @@ class Attachment(AbstractConstraint):
     def __init__(self,
                  parent_link: Link,
                  child_link: Link,
-                 bidirectional: Optional[bool] = False,
+                 bidirectional: bool = False,
                  parent_to_child_transform: Optional[Transform] = None,
-                 constraint_id: Optional[int] = None):
+                 constraint_id: Optional[int] = None,
+                 is_inverse: bool = False):
         """
         Creates an attachment between the parent object link and the child object link.
         This could be a bidirectional attachment, meaning that both objects will move when one moves.
@@ -220,6 +221,7 @@ def __init__(self,
         self.id = constraint_id
         self.bidirectional: bool = bidirectional
         self._loose: bool = False
+        self.is_inverse: bool = is_inverse
 
         if parent_to_child_transform is not None:
             self.parent_to_child_transform = parent_to_child_transform
@@ -283,7 +285,7 @@ def get_inverse(self) -> 'Attachment':
         :return: A new Attachment object with the parent and child links swapped.
         """
         attachment = Attachment(self.child_link, self.parent_link, self.bidirectional,
-                                constraint_id=self.id)
+                                constraint_id=self.id, is_inverse=not self.is_inverse)
         attachment.loose = not self._loose
         return attachment
 
@@ -303,13 +305,6 @@ def loose(self, loose: bool) -> None:
         """
         self._loose = loose and not self.bidirectional
 
-    @property
-    def is_reversed(self) -> bool:
-        """
-        :return: True if the parent and child links are swapped.
-        """
-        return self.loose
-
     def __del__(self) -> None:
         """
         Removes the constraint between the parent and the child links if one exists when the attachment is deleted.
@@ -326,9 +321,9 @@ def __eq__(self, other):
                 and self.bidirectional == other.bidirectional
                 and self.loose == other.loose
                 and np.allclose(self.parent_to_child_transform.translation_as_list(),
-                                other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-4)
+                                other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-3)
                 and np.allclose(self.parent_to_child_transform.rotation_as_list(),
-                                other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-4))
+                                other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-3))
 
     def __hash__(self):
         return hash((self.parent_link.name, self.child_link.name, self.bidirectional, self.parent_to_child_transform))

src/pycram/world_concepts/world_object.py

@@ -724,7 +724,7 @@ def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) ->
                     and not list(attachments.keys())[0].world.is_prospection_world:
                 obj = self.world.get_object_for_prospection_object(obj)
             if obj not in attachments:
-                if attachment.loose:
+                if attachment.is_inverse:
                     original_obj.detach(self)
                 else:
                     self.detach(original_obj)
@@ -739,10 +739,13 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) ->
                 obj = self.world.get_prospection_object_for_object(obj)
             if obj in self.attachments:
                 if self.attachments[obj] != attachment:
-                    self.detach(obj)
+                    if attachment.is_inverse:
+                        obj.detach(self)
+                    else:
+                        self.detach(obj)
                 else:
                     continue
-            if attachment.loose:
+            if attachment.is_inverse:
                 obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional)
             else:
                 self.attach(obj, attachment.parent_link.name, attachment.child_link.name,