Merge pull request cram2#157 from AbdelrhmanBassiouny/fix_attachment_…

…problem

Fix Attachment In Prospection

src/pycram/world.py

@@ -1109,7 +1109,8 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1):
         prospection world. When there are entries in the adding or removing queue the corresponding objects will
         be added or removed in the same iteration.
 
-        :param wait_time_as_n_simulation_steps: The time in simulation steps to wait between each iteration of the syncing loop.
+        :param wait_time_as_n_simulation_steps: The time in simulation steps to wait between each iteration of
+         the syncing loop.
         """
         while not self.terminate:
             self.check_for_pause()

src/pycram/world_concepts/constraints.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import numpy as np
 from geometry_msgs.msg import Point
 from typing_extensions import Union, List, Optional, TYPE_CHECKING
 
@@ -268,9 +269,13 @@ def __copy__(self):
                           self.id)
 
     def __eq__(self, other):
-        return (self.parent_link == other.parent_link and self.child_link == other.child_link and
-                self.bidirectional == other.bidirectional and
-                self.parent_to_child_transform == other.parent_to_child_transform)
+        return (self.parent_link.name == other.parent_link.name
+                and self.child_link.name == other.child_link.name
+                and self.bidirectional == other.bidirectional
+                and np.allclose(self.parent_to_child_transform.translation_as_list(),
+                                other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-4)
+                and np.allclose(self.parent_to_child_transform.rotation_as_list(),
+                                other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-4))
 
     def __hash__(self):
-        return hash((self.parent_link, self.child_link, self.bidirectional, self.parent_to_child_transform))
+        return hash((self.parent_link.name, self.child_link.name, self.bidirectional, self.parent_to_child_transform))

src/pycram/world_concepts/world_object.py

@@ -558,10 +558,27 @@ def current_state(self) -> ObjectState:
     def current_state(self, state: ObjectState) -> None:
         if self.get_pose().dist(state.pose) != 0.0:
             self.set_pose(state.pose, base=False, set_attachments=False)
-        self.attachments = state.attachments
+
+        self.set_attachments(state.attachments)
         self.link_states = state.link_states
         self.joint_states = state.joint_states
 
+    def set_attachments(self, attachments: Dict[Object, Attachment]) -> None:
+        """
+        Sets the attachments of this object to the given attachments.
+        :param attachments: A dictionary with the object as key and the attachment as value.
+        """
+        for obj, attachment in attachments.items():
+            if self.world.is_prospection_world and not obj.world.is_prospection_world:
+                obj = self.world.get_prospection_object_for_object(obj)
+            if obj in self.attachments:
+                if self.attachments[obj] != attachment:
+                    self.detach(obj)
+                else:
+                    continue
+            self.attach(obj, attachment.parent_link.name, attachment.child_link.name,
+                        attachment.bidirectional)
+
     @property
     def link_states(self) -> Dict[int, LinkState]:
         """
@@ -651,7 +668,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec
                 child.set_pose(link_to_object.to_pose(), set_attachments=False)
                 child._set_attached_objects_poses(already_moved_objects + [self])
 
-    def set_position(self, position: Union[Pose, Point], base=False) -> None:
+    def set_position(self, position: Union[Pose, Point, List], base=False) -> None:
         """
         Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position
         instead of the origin in the center of the Object. The given position can either be a Pose,
@@ -969,6 +986,17 @@ def get_base_origin(self) -> Pose:
         return Pose([aabb.min_x + base_width / 2, aabb.min_y + base_length / 2, aabb.min_z],
                     self.get_orientation_as_list())
 
+    def copy_to_prospection(self) -> Object:
+        """
+        Copies this object to the prospection world.
+
+        :return: The copied object in the prospection world.
+        """
+        obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(),
+                     self.world.prospection_world, self.color)
+        obj.current_state = self.current_state
+        return obj
+
     def __copy__(self) -> Object:
         """
         Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color.

test/test_attachment.py

@@ -1,4 +1,9 @@
+import time
+
 from bullet_world_testcase import BulletWorldTestCase
+from pycram.datastructures.enums import ObjectType
+from pycram.datastructures.pose import Pose
+from pycram.world_concepts.world_object import Object
 
 
 class TestAttachment(BulletWorldTestCase):
@@ -39,4 +44,50 @@ def test_detachment_behavior(self):
         new_milk_pos = self.milk.get_position()
         self.assertEqual(new_milk_pos.x, milk_pos.x)
 
+    def test_prospection_object_attachments_not_changed_with_real_object(self):
+        milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
+        cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl",
+                        pose=Pose([1.3, 0.7, 0.95]))
+        time.sleep(0.05)
+        milk_2.attach(cereal_2)
+        time.sleep(0.05)
+        prospection_milk = self.world.get_prospection_object_for_object(milk_2)
+        self.assertTrue(cereal_2 not in prospection_milk.attachments)
+        prospection_cereal = self.world.get_prospection_object_for_object(cereal_2)
+        self.assertTrue(prospection_cereal in prospection_milk.attachments)
+
+        # Assert that when prospection object is moved, the real object is not moved
+        prospection_milk_pos = prospection_milk.get_position()
+        cereal_pos = cereal_2.get_position()
+        prospection_cereal_pos = prospection_cereal.get_position()
+
+        # Move prospection milk object
+        prospection_milk_pos.x += 1
+        prospection_milk.set_position(prospection_milk_pos)
+
+        # Prospection Cereal object should move with prospection milk object
+        assumed_prospection_cereal_pos = prospection_cereal_pos
+        assumed_prospection_cereal_pos.x += 1
+        new_prospection_cereal_pos = prospection_cereal.get_position()
+        self.assertTrue(new_prospection_cereal_pos == assumed_prospection_cereal_pos)
+
+        # Real cereal object should not move
+        new_cereal_pos = cereal_2.get_position()
+        assumed_cereal_pos = cereal_pos
+        self.assertTrue(new_cereal_pos == assumed_cereal_pos)
+
+        self.world.remove_object(milk_2)
+        self.world.remove_object(cereal_2)
+
+    def test_attaching_to_robot_and_moving(self):
+        self.robot.attach(self.milk)
+        milk_pos = self.milk.get_position()
+        rob_pos = self.robot.get_position()
+
+        rob_pos.x += 1
+        self.robot.set_position(rob_pos)
+
+        new_milk_pos = self.milk.get_position()
+        self.assertEqual(new_milk_pos.x, milk_pos.x + 1)
+
 

test/test_bullet_world.py

@@ -132,7 +132,7 @@ def no_collision_callback():
         self.assertTrue(self.collision_called)
 
     def test_equal_world_states(self):
-        time.sleep(2)
+        time.sleep(2.5)
         self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1]))
         self.assertFalse(self.world.world_sync.check_for_equal())
         self.world.prospection_world.object_states = self.world.current_state.object_states
@@ -144,7 +144,7 @@ def test_add_resource_path(self):
         self.assertTrue("test" in self.world.data_directory)
 
     def test_no_prospection_object_found_for_given_object(self):
-        milk_2 = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
+        milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
         time.sleep(0.05)
         try:
             prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2)
@@ -156,7 +156,7 @@ def test_no_prospection_object_found_for_given_object(self):
             self.assertTrue(True)
 
     def test_no_object_found_for_given_prospection_object(self):
-        milk_2 = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
+        milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9]))
         time.sleep(0.05)
         prospection_milk = self.world.get_prospection_object_for_object(milk_2)
         self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2)
@@ -169,6 +169,36 @@ def test_no_object_found_for_given_prospection_object(self):
             self.assertTrue(True)
         time.sleep(0.05)
 
+    def test_real_object_position_does_not_change_with_prospection_object(self):
+        milk_2_pos = [1.3, 1, 0.9]
+        milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos))
+        time.sleep(0.05)
+        milk_2_pos = milk_2.get_position()
+        prospection_milk = self.world.get_prospection_object_for_object(milk_2)
+        prospection_milk_pos = prospection_milk.get_position()
+        self.assertTrue(prospection_milk_pos == milk_2_pos)
+
+        # Assert that when prospection object is moved, the real object is not moved
+        prospection_milk_pos.x += 1
+        prospection_milk.set_position(prospection_milk_pos)
+        self.assertTrue(prospection_milk.get_position() != milk_2.get_position())
+        self.world.remove_object(milk_2)
+
+    def test_prospection_object_position_does_not_change_with_real_object(self):
+        milk_2_pos = [1.3, 1, 0.9]
+        milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos))
+        time.sleep(0.05)
+        milk_2_pos = milk_2.get_position()
+        prospection_milk = self.world.get_prospection_object_for_object(milk_2)
+        prospection_milk_pos = prospection_milk.get_position()
+        self.assertTrue(prospection_milk_pos == milk_2_pos)
+
+        # Assert that when real object is moved, the prospection object is not moved
+        milk_2_pos.x += 1
+        milk_2.set_position(milk_2_pos)
+        self.assertTrue(prospection_milk.get_position() != milk_2.get_position())
+        self.world.remove_object(milk_2)
+
     def test_add_vis_axis(self):
         self.world.add_vis_axis(self.robot.get_link_pose(robot_description.get_camera_frame()))
         self.assertTrue(len(self.world.vis_axis) == 1)