diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 6c9ed8a73..537a92625 100644 --- a/src/pycram/datastructures/world.py +++ b/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 diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index a650049d7..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, 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): diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 676285f61..e00f6dfc6 100644 --- a/src/pycram/worlds/multiverse.py +++ b/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: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4e138fe02..693643221 100644 --- a/test/test_multiverse.py +++ b/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)