From 1ec5681fcdae5c9b3782fd4e920c0989a43b7443 Mon Sep 17 00:00:00 2001 From: mohammadkhoshnazarr Date: Thu, 10 Oct 2024 15:41:45 +0200 Subject: [PATCH 1/4] [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 054404d72..f5f9ec1f7 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -949,14 +949,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) @@ -964,7 +966,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) @@ -1004,11 +1007,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 @@ -1514,7 +1518,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 7f9f32b7a..a650049d7 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, 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 003bab86f..08da5213f 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, 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) From e154ff7f1850c4da5005441ce5a7a432b91f99db Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 12:13:22 +0200 Subject: [PATCH 2/4] [SaveState] Corrected arguments order in save simulator state. --- src/pycram/datastructures/world.py | 6 +++--- src/pycram/worlds/bullet_world.py | 2 +- src/pycram/worlds/multiverse.py | 20 ++++++++++---------- test/test_multiverse.py | 4 ++-- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index f5f9ec1f7..2b6879761 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -967,7 +967,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) @@ -1007,12 +1007,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 08da5213f..b4b65380e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -16,7 +16,7 @@ from ..description import Link, Joint, ObjectDescription 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 @@ -608,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 @@ -623,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): - logging.warning("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: - logging.warning("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]: - logging.warning("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: - logging.error("get_object_axis_aligned_bounding_box 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: - logging.error("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: - logging.warning("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") + 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: - logging.warning("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 6a44cd18d..3164de50a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -306,7 +306,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) @@ -320,7 +320,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) From 1b70ee41d4520caeaf4f398c5d348907bfff3e21 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 13:50:33 +0200 Subject: [PATCH 3/4] [SaveState] prevented manual saving of object states when use_physics_simulator_state configuration is set to True. --- src/pycram/datastructures/dataclasses.py | 13 ++++++---- src/pycram/datastructures/world.py | 30 +++++++++++++++++++----- 2 files changed, 33 insertions(+), 10 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 040189ce3..8f70b96f9 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -404,12 +404,17 @@ class WorldState(State): """ Dataclass for storing the state of the world. """ - simulator_state_id: Optional[int] - object_states: Dict[str, ObjectState] + simulator_state_id: Optional[int] = None + object_states: Optional[Dict[str, ObjectState]] = None def __eq__(self, other: 'WorldState'): - return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) - and self.all_objects_states_are_equal(other)) + if self.object_states is None and other.object_states is None: + return self.simulator_state_is_equal(other) + elif self.object_states is None or other.object_states is None: + return False + else: + return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) + and self.all_objects_states_are_equal(other)) def simulator_state_is_equal(self, other: 'WorldState') -> bool: """ diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 2b6879761..54d6de5a5 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -85,6 +85,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca """ StateEntity.__init__(self) + self.latest_state_id: Optional[int] = None if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): self.cache_manager.clear_cache() @@ -958,17 +959,33 @@ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) :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=state_id, use_same_id=use_same_id) - self.save_objects_state(state_id) - self._current_state = WorldState(state_id, self.object_states) + + sim_state_id = self.save_physics_simulator_state(state_id=state_id, use_same_id=use_same_id) + + if self.conf.use_physics_simulator_state: + object_states = None + else: + if state_id is None: + if self.latest_state_id is None: + self.latest_state_id = 0 + else: + self.latest_state_id += 0 if use_same_id else 1 + state_id = self.latest_state_id + + self.save_objects_state(state_id) + + object_states = self.object_states + + self._current_state = WorldState(sim_state_id, 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 ( + simulator_state_id = None if not 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) + self._current_state = WorldState(simulator_state_id, self.object_states) return WorldState(self._current_state.simulator_state_id, self.object_states) @current_state.setter @@ -1163,7 +1180,8 @@ def remove_saved_states(self) -> None: """ if self.conf.use_physics_simulator_state: for state_id in self.saved_states: - self.remove_physics_simulator_state(state_id) + if state_id is not None: + self.remove_physics_simulator_state(state_id) else: self.remove_objects_saved_states() super().remove_saved_states() From f90dcc9439c7ddef688a5d33756142d498d3c3e2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 19:20:30 +0200 Subject: [PATCH 4/4] [SaveState] save and restore joint states manually when using physics simulator restore state as the simulator does not take into account the joint values. --- src/pycram/datastructures/dataclasses.py | 16 ++++------ src/pycram/datastructures/world.py | 40 ++++++++++++++---------- test/test_multiverse.py | 27 ++++++++++++++++ 3 files changed, 57 insertions(+), 26 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 8f70b96f9..628102651 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -404,17 +404,12 @@ class WorldState(State): """ Dataclass for storing the state of the world. """ + object_states: Dict[str, ObjectState] simulator_state_id: Optional[int] = None - object_states: Optional[Dict[str, ObjectState]] = None def __eq__(self, other: 'WorldState'): - if self.object_states is None and other.object_states is None: - return self.simulator_state_is_equal(other) - elif self.object_states is None or other.object_states is None: - return False - else: - return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) - and self.all_objects_states_are_equal(other)) + return (self.simulator_state_is_equal(other) and self.all_objects_exist(other) + and self.all_objects_states_are_equal(other)) def simulator_state_is_equal(self, other: 'WorldState') -> bool: """ @@ -447,8 +442,9 @@ def all_objects_states_are_equal(self, other: 'WorldState') -> bool: other.object_states.values())]) def __copy__(self): - return WorldState(simulator_state_id=self.simulator_state_id, - object_states=deepcopy(self.object_states)) + return WorldState(object_states=deepcopy(self.object_states), + simulator_state_id=self.simulator_state_id + ) @dataclass diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 54d6de5a5..ba0703142 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -962,21 +962,16 @@ def save_state(self, state_id: Optional[int] = None, use_same_id: bool = False) sim_state_id = self.save_physics_simulator_state(state_id=state_id, use_same_id=use_same_id) - if self.conf.use_physics_simulator_state: - object_states = None - else: - if state_id is None: - if self.latest_state_id is None: - self.latest_state_id = 0 - else: - self.latest_state_id += 0 if use_same_id else 1 - state_id = self.latest_state_id - - self.save_objects_state(state_id) + if state_id is None: + if self.latest_state_id is None: + self.latest_state_id = 0 + else: + self.latest_state_id += 0 if use_same_id else 1 + state_id = self.latest_state_id - object_states = self.object_states + self.save_objects_state(state_id) - self._current_state = WorldState(sim_state_id, object_states) + self._current_state = WorldState(self.object_states, sim_state_id) return super().save_state(state_id) @@ -985,18 +980,31 @@ def current_state(self) -> WorldState: if self._current_state is None: simulator_state_id = None if not self.conf.use_physics_simulator_state else ( self.save_physics_simulator_state(use_same_id=True)) - self._current_state = WorldState(simulator_state_id, self.object_states) - return WorldState(self._current_state.simulator_state_id, self.object_states) + self._current_state = WorldState(self.object_states, simulator_state_id) + return WorldState(self.object_states, self._current_state.simulator_state_id) @current_state.setter def current_state(self, state: WorldState) -> None: if self.current_state != state: if self.conf.use_physics_simulator_state: self.restore_physics_simulator_state(state.simulator_state_id) + self.set_object_states_without_poses(state.object_states) else: for obj in self.objects: self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] + def set_object_states_without_poses(self, states: Dict[str, ObjectState]) -> None: + """ + Set the states of all objects in the World except the poses. + + :param states: A dictionary with the object id as key and the object state as value. + """ + for obj_name, obj_state in states.items(): + obj = self.get_object_by_name(obj_name) + obj.set_attachments(obj_state.attachments) + obj.link_states = obj_state.link_states + obj.joint_states = obj_state.joint_states + @property def object_states(self) -> Dict[str, ObjectState]: """ @@ -1172,7 +1180,7 @@ def reset_world(self, remove_saved_states=False) -> None: self.restore_state(self.original_state_id) if remove_saved_states: self.remove_saved_states() - self.original_state_id = self.save_state() + self.save_state(use_same_id=True) def remove_saved_states(self) -> None: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 3164de50a..693c54284 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -53,6 +53,33 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.remove_all_objects() + def test_save_and_restore_state(self): + milk = self.spawn_milk([1, 1, 0.1]) + robot = self.spawn_robot() + cup = self.spawn_cup([1, 2, 0.1]) + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + apartment.set_joint_position("cabinet10_drawer1_joint", 0.1) + robot.attach(milk) + milk.attach(cup) + all_object_attachments = {obj: obj.attachments.copy() for obj in self.multiverse.objects} + state_id = self.multiverse.save_state() + milk.detach(cup) + robot_link = robot.root_link + milk_link = milk.root_link + cid = robot_link.constraint_ids[milk_link] + self.assertTrue(cid == robot.attachments[milk].id) + self.multiverse.remove_constraint(cid) + apartment.set_joint_position("cabinet10_drawer1_joint", 0.0) + self.multiverse.restore_state(state_id) + cid = robot_link.constraint_ids[milk_link] + self.assertTrue(milk_link in robot_link.constraint_ids) + self.assertTrue(cid == robot.attachments[milk].id) + for obj in self.multiverse.objects: + self.assertTrue(len(obj.attachments) == len(all_object_attachments[obj])) + for att in obj.attachments: + self.assertTrue(att in all_object_attachments[obj]) + self.assertTrue(apartment.get_joint_position("cabinet10_drawer1_joint") == 0.1) + 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]))