Merge pull request #1 from mkhoshnam/user_save_state

User save state

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/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,11 +1049,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
@@ -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:

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, 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):

src/pycram/worlds/multiverse.py

@@ -610,11 +610,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)

test/test_multiverse.py

@@ -53,6 +53,10 @@ def tearDownClass(cls):
     def tearDown(self):
         self.multiverse.remove_all_objects()
 
+    def test_spawn_xml_object(self):
+        bread = Object("bread_1", ObjectType.GENERIC_OBJECT, "bread_1.xml", pose=Pose([1, 1, 0.1]))
+        self.assert_poses_are_equal(bread.get_pose(), Pose([1, 1, 0.1]))
+
     def test_get_axis_aligned_bounding_box_for_one_link_object(self):
         position = [1, 1, 0.1]
         milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1],