From 27c557bec481c0d435d92f80275ca40efcc9043b Mon Sep 17 00:00:00 2001 From: mohammadkhoshnazarr Date: Thu, 10 Oct 2024 15:41:45 +0200 Subject: [PATCH] [SaveWorldState] Corrected the save state functionality and added the ability for the used to specift the state id. --- demos/pycram_multiverse_demo/demo.py | 1 - src/pycram/datastructures/world.py | 14 +++++++++----- src/pycram/worlds/bullet_world.py | 2 +- src/pycram/worlds/multiverse.py | 12 +++++++----- 4 files changed, 17 insertions(+), 12 deletions(-) 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/world.py b/src/pycram/datastructures/world.py index 2a2c4d397..0a1011f29 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -990,14 +990,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) @@ -1005,7 +1007,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) @@ -1045,11 +1048,12 @@ 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, use_same_id: bool = False, state_id: Optional[int] = None) -> int: """ Save the state of the physics simulator and returns the unique id of the state. :param use_same_id: If the same id should be used for the state. + :param state_id: The used specified unique id representing the state. :return: The unique id representing the state. """ pass @@ -1555,7 +1559,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: diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index c80e925f1..90d1e1338 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -308,7 +308,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, use_same_id: bool = False, state_id: Optional[int] = None) -> 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 52dbb98c5..a11a435fe 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -609,11 +609,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, use_same_id: bool = False, state_id: Optional[int] = None) -> 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)