diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index cdc625dac..6e71ec112 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/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])) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index e3e8490a9..f8da52a6a 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/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__() diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 4ca2ea986..a72b3629c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -991,14 +991,16 @@ 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) @@ -1006,7 +1008,8 @@ def save_state(self, state_id: Optional[int] = None) -> int: @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: """ diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 939bafd04..fb2f78927 100644 --- a/src/pycram/helper.py +++ b/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 diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 7f9f32b7a..90851e466 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/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): diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a5e968a7a..6cbeb0d78 100644 --- a/src/pycram/worlds/multiverse.py +++ b/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) diff --git a/test/test_attachment.py b/test/test_attachment.py index a6491eece..bf8487942 100644 --- a/test/test_attachment.py +++ b/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) - - diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 1f8ed3725..309ac6c65 100644 --- a/test/test_multiverse.py +++ b/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)