[SaveState] Corrected arguments order in save simulator state.

src/pycram/datastructures/world.py

@@ -1009,7 +1009,7 @@ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False)
     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(use_same_id=True))
+                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)
 
@@ -1049,12 +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, state_id: Optional[int] = None) -> 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 use_same_id: If the same id should be used for 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.
         """
         pass

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, state_id: Optional[int] = None) -> 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

@@ -2,8 +2,6 @@
 from time import sleep
 
 import numpy as np
-import rospy
-import trimesh
 from tf.transformations import quaternion_matrix
 from typing_extensions import List, Dict, Optional, Union, Tuple
 
@@ -18,7 +16,7 @@
 from ..description import Link, Joint
 from ..object_descriptors.mjcf import ObjectDescription as MJCF
 from ..robot_description import RobotDescription
-from ..ros.logging import logwarn
+from ..ros.logging import logwarn, logerr
 from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz
 from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \
     validate_joint_position, validate_multiple_object_poses
@@ -610,7 +608,7 @@ def step(self):
             sleep(self.simulation_time_step)
             self.api_requester.pause_simulation()
 
-    def save_physics_simulator_state(self, use_same_id: bool = False, state_id: Optional[int] = None) -> int:
+    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
@@ -625,30 +623,30 @@ def restore_physics_simulator_state(self, state_id: int) -> None:
         self.api_requester.load(self.saved_simulator_states[state_id])
 
     def set_link_color(self, link: Link, rgba_color: Color):
-        rospy.logwarn("set_link_color is not implemented in Multiverse")
+        logwarn("set_link_color is not implemented in Multiverse")
 
     def get_link_color(self, link: Link) -> Color:
-        rospy.logwarn("get_link_color is not implemented in Multiverse")
+        logwarn("get_link_color is not implemented in Multiverse")
         return Color()
 
     def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]:
-        rospy.logwarn("get_colors_of_object_links is not implemented in Multiverse")
+        logwarn("get_colors_of_object_links is not implemented in Multiverse")
         return {}
 
     def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox:
-        rospy.logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse")
+        logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse")
         raise NotImplementedError
 
     def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox:
-        rospy.logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse")
+        logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse")
         raise NotImplementedError
 
     def set_realtime(self, real_time: bool) -> None:
-        rospy.logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the"
+        logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the"
                       "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time")
 
     def set_gravity(self, gravity_vector: List[float]) -> None:
-        rospy.logwarn("set_gravity is not implemented in Multiverse")
+        logwarn("set_gravity is not implemented in Multiverse")
 
     def check_object_exists(self, obj: Object) -> bool:
         """

test/test_multiverse.py

@@ -338,7 +338,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)
@@ -352,7 +352,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)