Merge remote-tracking branch 'original_pycram_repo/dev' into mesh_and…

…_object

# Conflicts:
#	src/pycram/worlds/multiverse.py

demos/pycram_multiverse_demo/demo.py

@@ -21,7 +21,6 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose):
 
     return object_desig
 
-
 world = Multiverse(simulation_name='pycram_test')
 extension = ObjectDescription.get_file_extension()
 robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01]))

src/pycram/datastructures/dataclasses.py

@@ -740,7 +740,7 @@ def get_objects_that_have_points(self) -> List[Object]:
         return list({point.link_b.object for point in self})
 
     def __str__(self):
-        return f"ContactPointsList: {', '.join(self.get_names_of_objects_that_have_points())}"
+        return f"ContactPointsList: {', '.join([point.__str__() for point in self])}"
 
     def __repr__(self):
         return self.__str__()

src/pycram/datastructures/world.py

@@ -991,22 +991,25 @@ def terminate_world_sync(self) -> None:
         self.resume_world_sync()
         self.world_sync.join()
 
-    def save_state(self, state_id: Optional[int] = None) -> int:
+    def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
         """
         Return the id of the saved state of the World. The saved state contains the states of all the objects and
         the state of the physics simulator.
 
+        :param state_id: The id of the saved state.
+        :param use_same_id: Whether to use the same current state id for the new saved state.
         :return: A unique id of the state
         """
-        state_id = self.save_physics_simulator_state()
+        state_id = self.save_physics_simulator_state(state_id=state_id, use_same_id=use_same_id)
         self.save_objects_state(state_id)
         self._current_state = WorldState(state_id, self.object_states)
         return super().save_state(state_id)
 
     @property
     def current_state(self) -> WorldState:
         if self._current_state is None:
-            simulator_state = None if self.conf.use_physics_simulator_state else self.save_physics_simulator_state(True)
+            simulator_state = None if self.conf.use_physics_simulator_state else (
+                self.save_physics_simulator_state(use_same_id=True))
             self._current_state = WorldState(simulator_state, self.object_states)
         return WorldState(self._current_state.simulator_state_id, self.object_states)
 
@@ -1046,10 +1049,11 @@ def save_objects_state(self, state_id: int) -> None:
             obj.save_state(state_id)
 
     @abstractmethod
-    def save_physics_simulator_state(self, use_same_id: bool = False) -> int:
+    def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
         """
         Save the state of the physics simulator and returns the unique id of the state.
 
+        :param state_id: The used specified unique id representing the state.
         :param use_same_id: If the same id should be used for the state.
         :return: The unique id representing the state.
         """
@@ -1556,7 +1560,7 @@ def update_simulator_state_id_in_original_state(self, use_same_id: bool = False)
         :param use_same_id: If the same id should be used for the state.
         """
         if self.conf.use_physics_simulator_state:
-            self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id)
+            self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=use_same_id)
 
     @property
     def original_state(self) -> WorldState:
@@ -1578,10 +1582,7 @@ class UseProspectionWorld:
         with UseProspectionWorld():
             NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform()
     """
-    WAIT_TIME_AS_N_SIMULATION_STEPS: int = 20
-    """
-    The time in simulation steps to wait before switching to the prospection world
-    """
+
 
     def __init__(self):
         self.prev_world: Optional[World] = None
@@ -1591,12 +1592,12 @@ def __enter__(self):
         """
         This method is called when entering the with block, it will set the current world to the prospection world
         """
+        # Please do not edit this function, it works as it is now!
         if not World.current_world.is_prospection_world:
             self.prev_world = World.current_world
             World.current_world = World.current_world.prospection_world
-            World.current_world.resume_world_sync()
-            time.sleep(self.WAIT_TIME_AS_N_SIMULATION_STEPS * World.current_world.simulation_time_step)
-            World.current_world.pause_world_sync()
+            # This is also a join statement since it is called from the main thread.
+            World.current_world.world_sync.sync_worlds()
 
     def __exit__(self, *args):
         """
@@ -1698,7 +1699,8 @@ def add_objects_not_in_prospection_world(self):
         """
         Adds all objects that are in the main world but not in the prospection world to the prospection world.
         """
-        [self.add_object(obj) for obj in self.world.objects if obj not in self.object_to_prospection_object_map]
+        obj_map_copy = copy(self.object_to_prospection_object_map)
+        [self.add_object(obj) for obj in self.world.objects if obj not in obj_map_copy.keys()]
 
     def add_object(self, obj: Object) -> None:
         """

src/pycram/helper.py

@@ -100,6 +100,7 @@ def find_multiverse_path() -> Optional[str]:
     """
     # Get the value of PYTHONPATH environment variable
     pythonpath = os.getenv('PYTHONPATH')
+    multiverse_relative_path = "Multiverse/multiverse"
 
     # Check if PYTHONPATH is set
     if pythonpath:
@@ -108,8 +109,8 @@ def find_multiverse_path() -> Optional[str]:
 
         # Iterate through each path and check if 'Multiverse' is in it
         for path in paths:
-            if 'multiverse' in path:
-                multiverse_path = path.split('multiverse')[0]
-                return multiverse_path + 'multiverse'
+            if multiverse_relative_path in path:
+                multiverse_path = path.split(multiverse_relative_path)[0]
+                return multiverse_path + multiverse_relative_path
 
 

src/pycram/worlds/bullet_world.py

@@ -302,7 +302,7 @@ def join_gui_thread_if_exists(self):
         if self._gui_thread:
             self._gui_thread.join()
 
-    def save_physics_simulator_state(self, use_same_id: bool = False) -> int:
+    def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
         return p.saveState(physicsClientId=self.id)
 
     def restore_physics_simulator_state(self, state_id):

src/pycram/worlds/multiverse.py

@@ -608,11 +608,13 @@ def step(self):
             sleep(self.simulation_time_step)
             self.api_requester.pause_simulation()
 
-    def save_physics_simulator_state(self, use_same_id: bool = False) -> int:
-        self.latest_save_id = 0 if self.latest_save_id is None else self.latest_save_id + int(not use_same_id)
-        save_name = f"save_{self.latest_save_id}"
-        self.saved_simulator_states[self.latest_save_id] = self.api_requester.save(save_name)
-        return self.latest_save_id
+    def save_physics_simulator_state(self, state_id: Optional[int] = None, use_same_id: bool = False) -> int:
+        if state_id is None:
+            self.latest_save_id = 0 if self.latest_save_id is None else self.latest_save_id + int(not use_same_id)
+            state_id = self.latest_save_id
+        save_name = f"save_{state_id}"
+        self.saved_simulator_states[state_id] = self.api_requester.save(save_name)
+        return state_id
 
     def remove_physics_simulator_state(self, state_id: int) -> None:
         self.saved_simulator_states.pop(state_id)

test/test_attachment.py

@@ -26,12 +26,13 @@ def test_detach_sync_in_prospection_world(self):
             pass
         self.milk.detach(self.robot)
         with UseProspectionWorld():
-            self.assertTrue(self.milk not in self.robot.attachments)
-            self.assertTrue(self.robot not in self.milk.attachments)
-            prospection_milk = self.world.get_prospection_object_for_object(self.milk)
-            prospection_robot = self.world.get_prospection_object_for_object(self.robot)
-            self.assertTrue(prospection_milk not in prospection_robot.attachments)
-            self.assertTrue(prospection_robot not in prospection_milk.attachments)
+            pass
+        self.assertTrue(self.milk not in self.robot.attachments)
+        self.assertTrue(self.robot not in self.milk.attachments)
+        prospection_milk = self.world.get_prospection_object_for_object(self.milk)
+        prospection_robot = self.world.get_prospection_object_for_object(self.robot)
+        self.assertTrue(prospection_milk not in prospection_robot.attachments)
+        self.assertTrue(prospection_robot not in prospection_milk.attachments)
 
     def test_attachment_behavior(self):
         self.robot.attach(self.milk)
@@ -104,5 +105,3 @@ def test_attaching_to_robot_and_moving(self):
 
         new_milk_pos = self.milk.get_position()
         self.assertEqual(new_milk_pos.x, milk_pos.x + 1)
-
-

test/test_multiverse.py

@@ -334,7 +334,7 @@ def test_get_object_contact_points(self):
             self.assertEqual(len(contact_points), 1)
             self.assertIsInstance(contact_points[0], ContactPoint)
             self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor)
-            cup = self.spawn_cup([1, 1, 0.2])
+            cup = self.spawn_cup([1, 1, 0.15])
             # This is needed because the cup is spawned in the air, so it needs to fall
             # to get in contact with the milk
             self.multiverse.simulate(0.3)
@@ -348,7 +348,7 @@ def test_get_object_contact_points(self):
     def test_get_contact_points_between_two_objects(self):
         for i in range(3):
             milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707])
-            cup = self.spawn_cup([1, 1, 0.2])
+            cup = self.spawn_cup([1, 1, 0.15])
             # This is needed because the cup is spawned in the air so it needs to fall
             # to get in contact with the milk
             self.multiverse.simulate(0.3)