diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 7da4ace8c..9f504a840 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/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)) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a41123915..2749e975d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/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,