From c9168b727557065153e879d714b1c5084461a104 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 27 Feb 2024 22:15:07 +0100 Subject: [PATCH 001/189] [MultiverseInterface] Created the multliverse interface and a test for it. --- src/pycram/worlds/multiverse.py | 247 ++++++++++++++++++++++++++++++++ test/test_multiverse.py | 73 ++++++++++ 2 files changed, 320 insertions(+) create mode 100644 src/pycram/worlds/multiverse.py create mode 100644 test/test_multiverse.py diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py new file mode 100644 index 000000000..335483553 --- /dev/null +++ b/src/pycram/worlds/multiverse.py @@ -0,0 +1,247 @@ +import logging +import os +import time +from pathlib import Path + +from typing_extensions import List, Dict, Optional + +from multiverse_pycram_socket.multiverse_socket import MultiverseSocket, SocketAddress + +from .datastructures.enums import WorldMode, JointType +from .datastructures.pose import Pose +from .datastructures.dataclasses import AxisAlignedBoundingBox, Color +from .concepts.world_object import Object +from .concepts.constraints import Constraint +from ..world import World +from ..description import Link, Joint + + +def get_resource_paths(dirname: str) -> List[str]: + + # resources_paths = ["../robots", "../worlds", "../objects"] + resources_paths = [] + resources_paths = [ + os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( + resources_path) else resources_path + for resources_path in resources_paths + ] + + def add_directories(path: str) -> List[str]: + with os.scandir(path) as entries: + for entry in entries: + if entry.is_dir(): + resources_paths.append(entry.path) + add_directories(entry.path) + + resources_path_copy = resources_paths.copy() + for resources_path in resources_path_copy: + add_directories(resources_path) + + return resources_paths + + +class Multiverse(MultiverseSocket, World): + """ + This class implements an interface between Multiverse and PyCRAM. + """ + + _joint_type_to_attribute: Dict[JointType, str] = { + JointType.REVOLUTE: "joint_rvalue", + JointType.PRISMATIC: "joint_tvalue", + } + """ + A dictionary to map JointType to the corresponding multiverse attribute name. + """ + + added_multiverse_resources: bool = False + """ + A flag to check if the multiverse resources have been added. + """ + + def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT, + is_prospection: Optional[bool] = False, + simulation_frequency: Optional[float] = 60.0, + client_addr: Optional[SocketAddress] = SocketAddress(port="7000")): + """ + Initialize the Multiverse Socket and the PyCram World. + param mode: The mode of the world (DIRECT or GUI). + param is_prospection: Whether the world is prospection or not. + param simulation_frequency: The frequency of the simulation. + param client_addr: The address of the multiverse client. + """ + MultiverseSocket.__init__(self, client_addr) + World.__init__(self, mode, is_prospection, simulation_frequency) + self.simulation: str = simulation + self._make_sure_multiverse_resources_are_added() + self.last_object_id: int = -1 + self.run() + + def _make_sure_multiverse_resources_are_added(self): + """ + Add the multiverse resources to the pycram world resources. + """ + if not self.added_multiverse_resources: + dirname = Path("../../../../../resources").resolve().__str__() + resources_paths = get_resource_paths(dirname) + for resource_path in resources_paths: + self.add_resource_path(resource_path) + self.added_multiverse_resources = True + + def get_joint_attribute(self, joint: Joint) -> str: + if joint.type not in self._joint_type_to_attribute: + logging.warning(f"Invalid joint type: {joint.type}") + return "joint_rvalue" + return self._joint_type_to_attribute[joint.type] + + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + """ + This is a placeholder until a proper spawning mechanism is available in Multiverse. + param path: The path is used as the name of the object. + param pose: The pose of the object. + """ + if pose is None: + pose = Pose() + self._reset_body_pose(path, pose) + self.last_object_id += 1 + return self.last_object_id + + def get_object_joint_names(self, obj: Object) -> List[str]: + return [joint.name for joint in obj.description.joints] + + def get_object_link_names(self, obj: Object) -> List[str]: + return [link.name for link in obj.description.links] + + def remove_object_from_simulator(self, obj: Object) -> None: + logging.warning("remove_object_from_simulator is not implemented in Multiverse") + + def add_constraint(self, constraint: Constraint) -> int: + logging.warning("add_constraint is not implemented in Multiverse") + return 0 + + def remove_constraint(self, constraint_id) -> None: + logging.warning("remove_constraint is not implemented in Multiverse") + + def _init_getter(self): + self.request_meta_data["receive"] = {} + self.request_meta_data["send"] = {} + self.request_meta_data["meta_data"]["simulation_name"] = self._meta_data.simulation_name + + def get_joint_position(self, joint: Joint) -> float: + self._init_getter() + attribute = self.get_joint_attribute(joint) + self.request_meta_data["receive"][joint.name] = [attribute] + self._communicate(True) + receive_data = self.response_meta_data["receive"][joint.name][attribute] + if len(receive_data) != 1: + logging.error(f"Invalid joint position data: {receive_data}") + raise ValueError + return receive_data[0] + + def _init_setter(self): + self.request_meta_data["send"] = {} + self.request_meta_data["receive"] = {} + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation + + def reset_joint_position(self, joint: Joint, joint_position: float) -> None: + self._init_setter() + self._communicate(True) + attribute = self.get_joint_attribute(joint) + self.request_meta_data["send"][joint.name] = [attribute] + self.send_data = [time.time(), joint_position] + self._communicate() + + def get_link_pose(self, link: Link) -> Pose: + return self._get_body_pose(link.name) + + def get_object_pose(self, obj: Object) -> Pose: + return self._get_body_pose(obj.name) + + def _get_body_pose(self, body_name: str) -> Pose: + self._init_getter() + self.request_meta_data["receive"][body_name] = ["position", "quaternion"] + self._communicate(True) + self._communicate() + if len(self.receive_data) != 8: + logging.error(f"Invalid body pose data: {self.receive_data}") + raise ValueError + return Pose(self.receive_data[1:4], self.receive_data[4:]) + + def reset_object_base_pose(self, obj: Object, pose: Pose): + self._reset_body_pose(obj.name, pose) + + def _reset_body_pose(self, body_name: str, pose: Pose): + """ + Reset the pose of a body in the simulator. + """ + self.request_meta_data["send"] = {} + self.request_meta_data["receive"] = {} + self.request_meta_data["meta_data"]["simulation_name"] = "crane_simulation" + self.request_meta_data["send"][body_name] = ["position", "quaternion"] + self._communicate(True) + self.send_data = [time.time(), *pose.position_as_list(), *pose.orientation_as_list()] + self._communicate(False) + + def perform_collision_detection(self) -> None: + logging.warning("perform_collision_detection is not implemented in Multiverse") + + def get_object_contact_points(self, obj: Object) -> List: + logging.warning("get_object_contact_points is not implemented in Multiverse") + return [] + + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") + return [] + + def step(self): + logging.warning("step is not implemented in Multiverse") + + def set_link_color(self, link: Link, rgba_color: Color): + logging.warning("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") + 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") + 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") + 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") + raise NotImplementedError + + def set_realtime(self, real_time: bool) -> None: + logging.warning("set_realtime is not implemented in Multiverse") + + def set_gravity(self, gravity_vector: List[float]) -> None: + logging.warning("set_gravity is not implemented in Multiverse") + + def disconnect_from_physics_server(self) -> None: + self.stop() + + def join_threads(self) -> None: + pass + + def save_physics_simulator_state(self) -> int: + logging.warning("save_physics_simulator_state is not implemented in Multiverse") + return 0 + + def remove_physics_simulator_state(self, state_id: int) -> None: + logging.warning("remove_physics_simulator_state is not implemented in Multiverse") + + def restore_physics_simulator_state(self, state_id: int) -> None: + logging.error("restore_physics_simulator_state is not implemented in Multiverse") + raise NotImplementedError + + def ray_test(self, from_position: List[float], to_position: List[float]) -> int: + logging.error("ray_test is not implemented in Multiverse") + raise NotImplementedError + + def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], + num_threads: int = 1) -> List[int]: + logging.error("ray_test_batch is not implemented in Multiverse") + raise NotImplementedError diff --git a/test/test_multiverse.py b/test/test_multiverse.py new file mode 100644 index 000000000..31dd8d498 --- /dev/null +++ b/test/test_multiverse.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +import time +import unittest + +from pycram.worlds.multiverse import Multiverse +from pycram.worlds.datastructures.enums import ObjectType +from pycram.worlds.datastructures.pose import Pose +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.worlds.concepts.world_object import Object + +multiverse_installed = True +try: + from multiverse_pycram_socket.multiverse_socket import SocketAddress +except ImportError: + multiverse_installed = False + + +@unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") +@unittest.skip("Needs Multiverse server and simulation to be running") +class MultiversePyCRAMTestCase(unittest.TestCase): + multiverse: Multiverse + + @classmethod + def setUpClass(cls): + cls.multiverse = Multiverse(simulation="crane_simulation", + client_addr=SocketAddress(port="5481"), + is_prospection=True) + + # cls.table = Object("wooden_log_1", ObjectType.GENERIC_OBJECT, "WoodenLog.stl", ObjectDescription, + # pose=Pose([-3.17, 4, 1], [0, 0, 0, 1])) + cls.robot = Object("ur5e", ObjectType.ROBOT, "ur5e_without_gripper.urdf", ObjectDescription, + pose=Pose()) + + @classmethod + def tearDownClass(cls): + cls.multiverse.disconnect_from_physics_server() + + @classmethod + def tearDown(cls): + # cls.table.set_position([-3.17, 4, 1]) + pass + + def test_set_position(self): + table_position = self.table.get_position_as_list() + self.assertEqual(table_position, [-3.17, 4, 1]) + table_position[0] += 1 + self.table.set_position(table_position) + table_position = self.table.get_position_as_list() + self.assertAlmostEqual(table_position, [-2.17, 4, 1]) + time.sleep(5) + + def test_update_position(self): + self.table.update_pose() + table_position = self.table.get_position_as_list() + for i, v in enumerate([-3.17, 4, 1]): + self.assertAlmostEqual(table_position[i], v) + + def test_get_joint_position(self): + joint_position = self.robot.get_joint_position("shoulder_pan_joint") + self.assertAlmostEqual(joint_position, 0.0) + + def test_set_joint_position(self): + joint_position = self.robot.get_joint_position("shoulder_pan_joint") + self.robot.set_joint_position("shoulder_pan_joint", joint_position - 1.0) + self.robot.joints["shoulder_pan_joint"]._update_position() + joint_position = self.robot.get_joint_position("shoulder_pan_joint") + self.assertAlmostEqual(joint_position, -1.0) + + def test_set_robot_position(self): + self.robot.set_position([0, 0, 1]) + self.assertEqual(self.robot.get_position_as_list(), [0, 0, 1]) + From 0ec67f314e52ac790c252c3ae3e32513beffd2dc Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 11 Mar 2024 12:41:25 +0100 Subject: [PATCH 002/189] [MultiverseInterface] Reordered methods by priority. --- src/pycram/worlds/multiverse.py | 72 ++++++++++++++++----------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 335483553..ea7470aa6 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -111,16 +111,6 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] - def remove_object_from_simulator(self, obj: Object) -> None: - logging.warning("remove_object_from_simulator is not implemented in Multiverse") - - def add_constraint(self, constraint: Constraint) -> int: - logging.warning("add_constraint is not implemented in Multiverse") - return 0 - - def remove_constraint(self, constraint_id) -> None: - logging.warning("remove_constraint is not implemented in Multiverse") - def _init_getter(self): self.request_meta_data["receive"] = {} self.request_meta_data["send"] = {} @@ -181,6 +171,22 @@ def _reset_body_pose(self, body_name: str, pose: Pose): self.send_data = [time.time(), *pose.position_as_list(), *pose.orientation_as_list()] self._communicate(False) + def disconnect_from_physics_server(self) -> None: + self.stop() + + def join_threads(self) -> None: + pass + + def remove_object_from_simulator(self, obj: Object) -> None: + logging.warning("remove_object_from_simulator is not implemented in Multiverse") + + def add_constraint(self, constraint: Constraint) -> int: + logging.warning("add_constraint is not implemented in Multiverse") + return 0 + + def remove_constraint(self, constraint_id) -> None: + logging.warning("remove_constraint is not implemented in Multiverse") + def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") @@ -192,9 +198,29 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") return [] + def ray_test(self, from_position: List[float], to_position: List[float]) -> int: + logging.error("ray_test is not implemented in Multiverse") + raise NotImplementedError + + def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], + num_threads: int = 1) -> List[int]: + logging.error("ray_test_batch is not implemented in Multiverse") + raise NotImplementedError + def step(self): logging.warning("step is not implemented in Multiverse") + def save_physics_simulator_state(self) -> int: + logging.warning("save_physics_simulator_state is not implemented in Multiverse") + return 0 + + def remove_physics_simulator_state(self, state_id: int) -> None: + logging.warning("remove_physics_simulator_state is not implemented in Multiverse") + + def restore_physics_simulator_state(self, state_id: int) -> None: + logging.error("restore_physics_simulator_state is not implemented in Multiverse") + raise NotImplementedError + def set_link_color(self, link: Link, rgba_color: Color): logging.warning("set_link_color is not implemented in Multiverse") @@ -219,29 +245,3 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") - - def disconnect_from_physics_server(self) -> None: - self.stop() - - def join_threads(self) -> None: - pass - - def save_physics_simulator_state(self) -> int: - logging.warning("save_physics_simulator_state is not implemented in Multiverse") - return 0 - - def remove_physics_simulator_state(self, state_id: int) -> None: - logging.warning("remove_physics_simulator_state is not implemented in Multiverse") - - def restore_physics_simulator_state(self, state_id: int) -> None: - logging.error("restore_physics_simulator_state is not implemented in Multiverse") - raise NotImplementedError - - def ray_test(self, from_position: List[float], to_position: List[float]) -> int: - logging.error("ray_test is not implemented in Multiverse") - raise NotImplementedError - - def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], - num_threads: int = 1) -> List[int]: - logging.error("ray_test_batch is not implemented in Multiverse") - raise NotImplementedError From 9565035378970212c48617ac380d9563da1103c5 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 25 Mar 2024 13:42:31 +0100 Subject: [PATCH 003/189] [MultiverseIntegration] path is required when loading object. --- src/pycram/world.py | 4 +- .../world_concepts/multiverse_socket.py | 116 ++++++++++++++++++ src/pycram/worlds/bullet_world.py | 2 +- src/pycram/worlds/multiverse.py | 108 ++++++++++++---- test/test_multiverse.py | 18 +-- 5 files changed, 212 insertions(+), 36 deletions(-) create mode 100644 src/pycram/world_concepts/multiverse_socket.py diff --git a/src/pycram/world.py b/src/pycram/world.py index 97e4361f7..2ef6ca30c 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -241,11 +241,11 @@ def simulation_time_step(self): return 1 / World.simulation_frequency @abstractmethod - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: """ Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. - :param path: The path to the description file, if None the description file is assumed to be already loaded. + :param path: The path to the description file. :param pose: The pose at which the object should be loaded. :return: The id of the loaded object. """ diff --git a/src/pycram/world_concepts/multiverse_socket.py b/src/pycram/world_concepts/multiverse_socket.py new file mode 100644 index 000000000..66f327bd4 --- /dev/null +++ b/src/pycram/world_concepts/multiverse_socket.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 + +import dataclasses +from typing import List, Dict, TypeVar +import logging + +from multiverse_client_pybind import MultiverseClientPybind # noqa + +T = TypeVar("T") + + +@dataclasses.dataclass +class MultiverseMetaData: + world_name: str = "world" + simulation_name: str = "cram" + length_unit: str = "m" + angle_unit: str = "rad" + mass_unit: str = "kg" + time_unit: str = "s" + handedness: str = "rhs" + + +class SocketAddress: + host: str = "tcp://127.0.0.1" + port: str = "" + + def __init__(self, port: str) -> None: + self.port = port + + +class MultiverseSocket: + _server_addr: SocketAddress = SocketAddress(port="7000") + _client_addr: SocketAddress + _meta_data: MultiverseMetaData + _multiverse_socket: MultiverseClientPybind + + def __init__( + self, + client_addr: SocketAddress, + multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(), + ) -> None: + if not isinstance(client_addr.port, str) or client_addr.port == "": + raise ValueError(f"Must specify client port for {self.__class__.__name__}") + self._send_data = None + self._client_addr = client_addr + self._meta_data = multiverse_meta_data + self._multiverse_socket = MultiverseClientPybind( + f"{self._server_addr.host}:{self._server_addr.port}" + ) + self.request_meta_data = { + "meta_data": self._meta_data.__dict__, + "send": {}, + "receive": {}, + } + + def run(self) -> None: + message = f"[Client {self._client_addr.port}] Start {self.__class__.__name__}{self._client_addr.port}" + logging.info(message) + self._connect_and_start() + + def stop(self) -> None: + self._disconnect() + + def send_and_receive_meta_data(self): + self._communicate(True) + + def send_and_receive_data(self): + self._communicate(False) + + @property + def request_meta_data(self) -> Dict: + return self._request_meta_data + + @request_meta_data.setter + def request_meta_data(self, request_meta_data: Dict) -> None: + self._request_meta_data = request_meta_data + self._multiverse_socket.set_request_meta_data(self._request_meta_data) + + @property + def response_meta_data(self) -> Dict: + response_meta_data = self._multiverse_socket.get_response_meta_data() + if not response_meta_data: + message = f"[Client {self._client_addr.port}] Receive empty response meta data." + logging.warning(message) + return response_meta_data + + @property + def send_data(self) -> List[float]: + return self._send_data + + @send_data.setter + def send_data(self, send_data: List[float]) -> None: + self._send_data = send_data + self._multiverse_socket.set_send_data(self._send_data) + + @property + def receive_data(self) -> List[float]: + receive_data = self._multiverse_socket.get_receive_data() + if not receive_data: + message = f"[Client {self._client_addr.port}] Receive empty data." + logging.warning(message) + return receive_data + + def _connect_and_start(self) -> None: + self._multiverse_socket.connect(self._client_addr.host, self._client_addr.port) + self._multiverse_socket.start() + + def _disconnect(self) -> None: + self._multiverse_socket.disconnect() + + def _communicate(self, resend_request_meta_data: bool = False) -> None: + self._multiverse_socket.communicate(resend_request_meta_data) + + def _restart(self) -> None: + self._disconnect() + self._connect_and_start() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 0013a52d5..d43a76c1e 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -66,7 +66,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, world=self) - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: if pose is None: pose = Pose() return self._load_object_and_get_id(path, pose) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ea7470aa6..c9f1cd619 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,25 +1,21 @@ import logging import os -import time -from pathlib import Path +from time import time from typing_extensions import List, Dict, Optional -from multiverse_pycram_socket.multiverse_socket import MultiverseSocket, SocketAddress - -from .datastructures.enums import WorldMode, JointType -from .datastructures.pose import Pose -from .datastructures.dataclasses import AxisAlignedBoundingBox, Color -from .concepts.world_object import Object -from .concepts.constraints import Constraint -from ..world import World +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color +from ..datastructures.enums import WorldMode, JointType +from ..datastructures.pose import Pose from ..description import Link, Joint +from ..world import World +from ..world_concepts.constraints import Constraint +from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress +from ..world_concepts.world_object import Object def get_resource_paths(dirname: str) -> List[str]: - - # resources_paths = ["../robots", "../worlds", "../objects"] - resources_paths = [] + resources_paths = ["../robots", "../worlds", "../objects"] resources_paths = [ os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( resources_path) else resources_path @@ -40,12 +36,52 @@ def add_directories(path: str) -> List[str]: return resources_paths +def find_multiverse_resources_path() -> Optional[str]: + """ + Find the path to the Multiverse resources directory. + """ + # Get the path to the Multiverse installation + multiverse_path = find_multiverse_path() + + # Check if the path to the Multiverse installation was found + if multiverse_path: + # Construct the path to the resources directory + resources_path = os.path.join(multiverse_path, 'resources') + + # Check if the resources directory exists + if os.path.exists(resources_path): + return resources_path + + return None + + +def find_multiverse_path() -> Optional[str]: + """ + Find the path to the Multiverse installation. + """ + # Get the value of PYTHONPATH environment variable + pythonpath = os.getenv('PYTHONPATH') + + # Check if PYTHONPATH is set + if pythonpath: + # Split the PYTHONPATH into individual paths using the platform-specific path separator + paths = pythonpath.split(os.pathsep) + + # Iterate through each path and check if 'Multiverse' is in it + for path in paths: + if 'multiverse' in path: + multiverse_path = path.split('multiverse')[0] + return multiverse_path + 'multiverse' + + return None + + class Multiverse(MultiverseSocket, World): """ This class implements an interface between Multiverse and PyCRAM. """ - _joint_type_to_attribute: Dict[JointType, str] = { + _joint_type_to_position_name: Dict[JointType, str] = { JointType.REVOLUTE: "joint_rvalue", JointType.PRISMATIC: "joint_tvalue", } @@ -74,6 +110,7 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.simulation: str = simulation self._make_sure_multiverse_resources_are_added() self.last_object_id: int = -1 + self.time_start = time() self.run() def _make_sure_multiverse_resources_are_added(self): @@ -81,19 +118,19 @@ def _make_sure_multiverse_resources_are_added(self): Add the multiverse resources to the pycram world resources. """ if not self.added_multiverse_resources: - dirname = Path("../../../../../resources").resolve().__str__() + dirname = find_multiverse_resources_path() resources_paths = get_resource_paths(dirname) for resource_path in resources_paths: self.add_resource_path(resource_path) self.added_multiverse_resources = True - def get_joint_attribute(self, joint: Joint) -> str: - if joint.type not in self._joint_type_to_attribute: + def get_joint_position_name(self, joint: Joint) -> str: + if joint.type not in self._joint_type_to_position_name: logging.warning(f"Invalid joint type: {joint.type}") return "joint_rvalue" - return self._joint_type_to_attribute[joint.type] + return self._joint_type_to_position_name[joint.type] - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: """ This is a placeholder until a proper spawning mechanism is available in Multiverse. param path: The path is used as the name of the object. @@ -101,8 +138,27 @@ def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose """ if pose is None: pose = Pose() + + name = path.split('/')[-1] + self.request_meta_data["meta_data"]["simulation_name"] = self._meta_data.simulation_name + self.request_meta_data["send"][path] = ["position", + "quaternion", + "relative_velocity"] + self.send_and_receive_meta_data() + + time_now = time() - self.time_start + self.send_data = [time_now, + 0, 0, 5, + 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0, 0, 3, + 0.0, 0.0, 0.0, 1.0] + self.send_and_receive_data() + self._reset_body_pose(path, pose) + self.last_object_id += 1 + return self.last_object_id def get_object_joint_names(self, obj: Object) -> List[str]: @@ -118,9 +174,9 @@ def _init_getter(self): def get_joint_position(self, joint: Joint) -> float: self._init_getter() - attribute = self.get_joint_attribute(joint) + attribute = self.get_joint_position_name(joint) self.request_meta_data["receive"][joint.name] = [attribute] - self._communicate(True) + self.send_and_receive_meta_data() receive_data = self.response_meta_data["receive"][joint.name][attribute] if len(receive_data) != 1: logging.error(f"Invalid joint position data: {receive_data}") @@ -134,11 +190,11 @@ def _init_setter(self): def reset_joint_position(self, joint: Joint, joint_position: float) -> None: self._init_setter() - self._communicate(True) - attribute = self.get_joint_attribute(joint) + self.send_and_receive_meta_data() + attribute = self.get_joint_position_name(joint) self.request_meta_data["send"][joint.name] = [attribute] - self.send_data = [time.time(), joint_position] - self._communicate() + self.send_data = [time(), joint_position] + self.send_and_receive_data() def get_link_pose(self, link: Link) -> Pose: return self._get_body_pose(link.name) @@ -168,7 +224,7 @@ def _reset_body_pose(self, body_name: str, pose: Pose): self.request_meta_data["meta_data"]["simulation_name"] = "crane_simulation" self.request_meta_data["send"][body_name] = ["position", "quaternion"] self._communicate(True) - self.send_data = [time.time(), *pose.position_as_list(), *pose.orientation_as_list()] + self.send_data = [time(), *pose.position_as_list(), *pose.orientation_as_list()] self._communicate(False) def disconnect_from_physics_server(self) -> None: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 31dd8d498..da61b472d 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -3,23 +3,27 @@ import time import unittest +from typing_extensions import Optional + from pycram.worlds.multiverse import Multiverse -from pycram.worlds.datastructures.enums import ObjectType -from pycram.worlds.datastructures.pose import Pose +from pycram.datastructures.enums import ObjectType +from pycram.datastructures.pose import Pose from pycram.object_descriptors.urdf import ObjectDescription -from pycram.worlds.concepts.world_object import Object +from pycram.world_concepts.world_object import Object multiverse_installed = True try: - from multiverse_pycram_socket.multiverse_socket import SocketAddress + from pycram.world_concepts.multiverse_socket import MultiverseMetaData, SocketAddress except ImportError: multiverse_installed = False @unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") -@unittest.skip("Needs Multiverse server and simulation to be running") +# @unittest.skip("Needs Multiverse server and simulation to be running") class MultiversePyCRAMTestCase(unittest.TestCase): multiverse: Multiverse + table: Optional[Object] = None + robot: Optional[Object] = None @classmethod def setUpClass(cls): @@ -38,8 +42,8 @@ def tearDownClass(cls): @classmethod def tearDown(cls): - # cls.table.set_position([-3.17, 4, 1]) - pass + if cls.table is not None: + cls.table.set_position([-3.17, 4, 1]) def test_set_position(self): table_position = self.table.get_position_as_list() From eb0079587efb7f88d9217844d47c5f6d343a2468 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 25 Mar 2024 17:36:13 +0100 Subject: [PATCH 004/189] [MultiverseIntegration] spawning and destroying work but not for robots apparently. --- src/pycram/world.py | 4 +- src/pycram/world_concepts/world_object.py | 16 +++--- src/pycram/worlds/bullet_world.py | 4 +- src/pycram/worlds/multiverse.py | 62 ++++++++++---------- test/test_multiverse.py | 69 ++++++++++++++--------- 5 files changed, 86 insertions(+), 69 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 2ef6ca30c..f09a3ffa4 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -241,11 +241,11 @@ def simulation_time_step(self): return 1 / World.simulation_frequency @abstractmethod - def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, obj: Object, pose: Optional[Pose] = None) -> int: """ Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. - :param path: The path to the description file. + :param obj: The object to be loaded. :param pose: The pose at which the object should be loaded. :return: The id of the loaded object. """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 54a80f8ae..ce91e50b7 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -114,20 +114,18 @@ def _load_object_and_get_id(self, path: Optional[str] = None, """ if path is not None: try: - path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) except FileNotFoundError as e: logging.error("Could not generate description from file.") raise e + else: + self.path = self.name + try: - simulator_object_path = path - if simulator_object_path is None: - # This is useful when the object is already loaded in the simulator so it would use its name instead of - # its path - simulator_object_path = self.name - obj_id = self.world.load_object_and_get_id(simulator_object_path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) - return obj_id, path + obj_id = self.world.load_object_and_get_id(self, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) + return obj_id, self.path except Exception as e: logging.error( diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index d43a76c1e..73cad53ee 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -66,10 +66,10 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, world=self) - def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, obj: Object, pose: Optional[Pose] = None) -> int: if pose is None: pose = Pose() - return self._load_object_and_get_id(path, pose) + return self._load_object_and_get_id(obj.path, pose) def _load_object_and_get_id(self, path: str, pose: Pose) -> int: if path is None: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index c9f1cd619..24f81c253 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -110,6 +110,8 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.simulation: str = simulation self._make_sure_multiverse_resources_are_added() self.last_object_id: int = -1 + self.object_name_to_id: Dict[str, int] = {} + self.object_id_to_name: Dict[int, str] = {} self.time_start = time() self.run() @@ -130,37 +132,33 @@ def get_joint_position_name(self, joint: Joint) -> str: return "joint_rvalue" return self._joint_type_to_position_name[joint.type] - def load_object_and_get_id(self, path: str, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, obj: Object, pose: Optional[Pose] = None) -> int: """ - This is a placeholder until a proper spawning mechanism is available in Multiverse. - param path: The path is used as the name of the object. + Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as + the name of the object in the description file. + param obj: The object to be loaded. param pose: The pose of the object. """ if pose is None: pose = Pose() - name = path.split('/')[-1] - self.request_meta_data["meta_data"]["simulation_name"] = self._meta_data.simulation_name - self.request_meta_data["send"][path] = ["position", - "quaternion", - "relative_velocity"] - self.send_and_receive_meta_data() - - time_now = time() - self.time_start - self.send_data = [time_now, - 0, 0, 5, - 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0, 0, 3, - 0.0, 0.0, 0.0, 1.0] - self.send_and_receive_data() - - self._reset_body_pose(path, pose) + self._reset_body_pose(obj.name, pose) self.last_object_id += 1 + self.object_name_to_id[obj.name] = self.last_object_id + self.object_id_to_name[self.last_object_id] = obj.name + return self.last_object_id + def _init_spawn_object(self, name: str) -> None: + """ + Initialize the object spawning process. + """ + self._init_setter() + self.request_meta_data["send"][name] = ["position", "quaternion"] + self.send_and_receive_meta_data() + def get_object_joint_names(self, obj: Object) -> List[str]: return [joint.name for joint in obj.description.joints] @@ -186,6 +184,9 @@ def get_joint_position(self, joint: Joint) -> float: def _init_setter(self): self.request_meta_data["send"] = {} self.request_meta_data["receive"] = {} + self.set_simulation_in_request_meta_data() + + def set_simulation_in_request_meta_data(self): self.request_meta_data["meta_data"]["simulation_name"] = self.simulation def reset_joint_position(self, joint: Joint, joint_position: float) -> None: @@ -205,8 +206,8 @@ def get_object_pose(self, obj: Object) -> Pose: def _get_body_pose(self, body_name: str) -> Pose: self._init_getter() self.request_meta_data["receive"][body_name] = ["position", "quaternion"] - self._communicate(True) - self._communicate() + self.send_and_receive_meta_data() + self.send_and_receive_data() if len(self.receive_data) != 8: logging.error(f"Invalid body pose data: {self.receive_data}") raise ValueError @@ -219,13 +220,11 @@ def _reset_body_pose(self, body_name: str, pose: Pose): """ Reset the pose of a body in the simulator. """ - self.request_meta_data["send"] = {} - self.request_meta_data["receive"] = {} - self.request_meta_data["meta_data"]["simulation_name"] = "crane_simulation" + self._init_setter() self.request_meta_data["send"][body_name] = ["position", "quaternion"] - self._communicate(True) - self.send_data = [time(), *pose.position_as_list(), *pose.orientation_as_list()] - self._communicate(False) + self.send_and_receive_meta_data() + self.send_data = [time() - self.time_start, *pose.position_as_list(), *pose.orientation_as_list()] + self.send_and_receive_data() def disconnect_from_physics_server(self) -> None: self.stop() @@ -234,7 +233,12 @@ def join_threads(self) -> None: pass def remove_object_from_simulator(self, obj: Object) -> None: - logging.warning("remove_object_from_simulator is not implemented in Multiverse") + self.set_simulation_in_request_meta_data() + self.request_meta_data["send"][obj.name] = [] + self.request_meta_data["receive"][obj.name] = [] + self.send_and_receive_meta_data() + self.send_data = [time() - self.time_start] + self.send_and_receive_data() def add_constraint(self, constraint: Constraint) -> int: logging.warning("add_constraint is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index da61b472d..de94199e0 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,15 +1,13 @@ #!/usr/bin/env python3 -import time import unittest from typing_extensions import Optional -from pycram.worlds.multiverse import Multiverse from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose -from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse import Multiverse multiverse_installed = True try: @@ -22,49 +20,57 @@ # @unittest.skip("Needs Multiverse server and simulation to be running") class MultiversePyCRAMTestCase(unittest.TestCase): multiverse: Multiverse - table: Optional[Object] = None + wooden_log: Optional[Object] = None robot: Optional[Object] = None @classmethod def setUpClass(cls): - cls.multiverse = Multiverse(simulation="crane_simulation", + cls.multiverse = Multiverse(simulation="pycram_test", client_addr=SocketAddress(port="5481"), is_prospection=True) - - # cls.table = Object("wooden_log_1", ObjectType.GENERIC_OBJECT, "WoodenLog.stl", ObjectDescription, - # pose=Pose([-3.17, 4, 1], [0, 0, 0, 1])) - cls.robot = Object("ur5e", ObjectType.ROBOT, "ur5e_without_gripper.urdf", ObjectDescription, - pose=Pose()) + # cls.wooden_log = Object("wooden_log", ObjectType.GENERIC_OBJECT, "WoodenLog.urdf", + # pose=Pose([0, 0, 0.5], [0, 0, 0, 1])) @classmethod def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() - @classmethod - def tearDown(cls): - if cls.table is not None: - cls.table.set_position([-3.17, 4, 1]) + def tearDown(self): + self.multiverse.reset_world() + + def test_reset_world(self): + self.multiverse.reset_world() + + def test_spawn_object(self): + milk = self.spawn_milk() + + def test_remove_object(self): + milk = self.spawn_milk() + milk.remove() def test_set_position(self): - table_position = self.table.get_position_as_list() - self.assertEqual(table_position, [-3.17, 4, 1]) - table_position[0] += 1 - self.table.set_position(table_position) - table_position = self.table.get_position_as_list() - self.assertAlmostEqual(table_position, [-2.17, 4, 1]) - time.sleep(5) + milk = self.spawn_milk() + original_milk_position = milk.get_position_as_list() + original_milk_position[0] += 1 + milk.set_position(original_milk_position) + milk_position = milk.get_position_as_list() + self.assertAlmostEqual(milk_position, original_milk_position) def test_update_position(self): - self.table.update_pose() - table_position = self.table.get_position_as_list() - for i, v in enumerate([-3.17, 4, 1]): - self.assertAlmostEqual(table_position[i], v) + milk = self.spawn_milk() + milk.update_pose() + milk_position = milk.get_position_as_list() + for i, v in enumerate([0, 0, 2]): + self.assertAlmostEqual(milk_position[i], v) def test_get_joint_position(self): - joint_position = self.robot.get_joint_position("shoulder_pan_joint") - self.assertAlmostEqual(joint_position, 0.0) + # self.wooden_log.remove() + self.spawn_robot() + # joint_position = self.robot.get_joint_position("joint1") + # self.assertAlmostEqual(joint_position, 0.0) def test_set_joint_position(self): + self.spawn_robot() joint_position = self.robot.get_joint_position("shoulder_pan_joint") self.robot.set_joint_position("shoulder_pan_joint", joint_position - 1.0) self.robot.joints["shoulder_pan_joint"]._update_position() @@ -75,3 +81,12 @@ def test_set_robot_position(self): self.robot.set_position([0, 0, 1]) self.assertEqual(self.robot.get_position_as_list(), [0, 0, 1]) + @staticmethod + def spawn_milk() -> Object: + return Object("milk_box", ObjectType.MILK, "milk_box.urdf", + pose=Pose([0, 0, 2], [0, 0, 0, 1])) + + @staticmethod + def spawn_robot() -> Object: + return Object("panda", ObjectType.ROBOT, "panda.urdf", + pose=Pose([0, 0, 3], [0, 0, 0, 1])) From 23ee2bee44b4803b8b06af457ec541af2b1c46b9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 26 Mar 2024 17:28:12 +0100 Subject: [PATCH 005/189] [AbstractWorld] check_object_exists --- test/test_multiverse.py | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index de94199e0..ab04384ae 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 - +import time import unittest from typing_extensions import Optional @@ -20,7 +20,7 @@ # @unittest.skip("Needs Multiverse server and simulation to be running") class MultiversePyCRAMTestCase(unittest.TestCase): multiverse: Multiverse - wooden_log: Optional[Object] = None + big_bowl: Optional[Object] = None robot: Optional[Object] = None @classmethod @@ -28,25 +28,43 @@ def setUpClass(cls): cls.multiverse = Multiverse(simulation="pycram_test", client_addr=SocketAddress(port="5481"), is_prospection=True) - # cls.wooden_log = Object("wooden_log", ObjectType.GENERIC_OBJECT, "WoodenLog.urdf", - # pose=Pose([0, 0, 0.5], [0, 0, 0, 1])) + # cls.big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", + # pose=Pose([2, 2, 1], [0, 0, 0, 1])) @classmethod def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() def tearDown(self): - self.multiverse.reset_world() + # self.multiverse.reset_world() + pass def test_reset_world(self): + self.big_bowl.set_position([1, 1, 0]) + self.assertAlmostEqual(self.big_bowl.get_position_as_list(), [1, 1, 0]) self.multiverse.reset_world() + big_bowl_pose = self.big_bowl.get_pose() + self.assertAlmostEqual(big_bowl_pose, self.big_bowl.original_pose) def test_spawn_object(self): milk = self.spawn_milk() + self.assertIsInstance(milk, Object) + milk_pose = milk.get_pose() + self.assertAlmostEqual(milk_pose, milk.original_pose) def test_remove_object(self): milk = self.spawn_milk() milk.remove() + milk.set_position([1, 1, 1]) + + def test_check_object_exists(self): + self.multiverse.request_meta_data["send"] = {} + self.multiverse.request_meta_data["meta_data"]["world_name"] = "" + self.multiverse.request_meta_data["meta_data"]["simulation_name"] = self.multiverse._meta_data.simulation_name + self.multiverse.request_meta_data["receive"] = {} + print(self.multiverse.request_meta_data) + self.multiverse.send_and_receive_meta_data() + print(self.multiverse.response_meta_data) def test_set_position(self): milk = self.spawn_milk() From 82058c8428dcbb18896a383adf334c73a0c19503 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 28 Mar 2024 10:09:44 +0100 Subject: [PATCH 006/189] [AbstractWorld] check_object_exists --- src/pycram/world.py | 20 +++++++++++++----- src/pycram/world_concepts/world_object.py | 12 +++++++---- src/pycram/worlds/multiverse.py | 25 ++++++++++++++++++++++- test/test_multiverse.py | 23 ++++++++++----------- 4 files changed, 58 insertions(+), 22 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index f09a3ffa4..06dac388f 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -15,15 +15,16 @@ from typing_extensions import Union from pycram.cache_manager import CacheManager +from pycram.datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, + SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, State, WorldState) from pycram.datastructures.enums import JointType, ObjectType, WorldMode -from pycram.world_concepts.event import Event from pycram.datastructures.local_transformer import LocalTransformer from pycram.datastructures.pose import Pose, Transform from pycram.world_concepts.constraints import Constraint -from pycram.datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) +from pycram.world_concepts.event import Event if TYPE_CHECKING: from pycram.world_concepts.world_object import Object @@ -580,6 +581,15 @@ def robot_is_set() -> bool: """ return World.robot is not None + def check_object_exists_and_issue_warning_if_not(self, obj: Object) -> bool: + """ + Check if the object exists in the world and raise an error if it does not. + """ + if obj not in self.objects: + rospy.logwarn(f"Object {obj.name} not found in the world.") + return False + return True + def exit(self) -> None: """ Closes the World as well as the prospection world, also collects any other thread that is running. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index ce91e50b7..298baa5f4 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -2,6 +2,7 @@ import logging import os +from typing import Iterable import numpy as np import rospy @@ -621,7 +622,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec child.set_pose(link_to_object.to_pose(), set_attachments=False) child._set_attached_objects_poses(already_moved_objects + [self]) - def set_position(self, position: Union[Pose, Point], base=False) -> None: + def set_position(self, position: Union[Pose, Point, List], base=False) -> None: """ Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position instead of the origin in the center of the Object. The given position can either be a Pose, @@ -636,10 +637,13 @@ def set_position(self, position: Union[Pose, Point], base=False) -> None: pose.frame = position.frame elif isinstance(position, Point): target_position = position - elif isinstance(position, list): - target_position = position + elif isinstance(position, List): + if len(position) == 3: + target_position = Point(*position) + else: + raise ValueError("The given position has to be a list of 3 values.") else: - raise TypeError("The given position has to be a Pose, Point or a list of xyz.") + raise TypeError("The given position has to be a Pose, Point or an iterable of xyz values.") pose.position = target_position pose.orientation = self.get_orientation() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 24f81c253..5d50dc78c 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -171,6 +171,7 @@ def _init_getter(self): self.request_meta_data["meta_data"]["simulation_name"] = self._meta_data.simulation_name def get_joint_position(self, joint: Joint) -> float: + self.check_object_exists_and_issue_warning_if_not(joint.object) self._init_getter() attribute = self.get_joint_position_name(joint) self.request_meta_data["receive"][joint.name] = [attribute] @@ -198,9 +199,11 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: self.send_and_receive_data() def get_link_pose(self, link: Link) -> Pose: + self.check_object_exists_and_issue_warning_if_not(link.object) return self._get_body_pose(link.name) def get_object_pose(self, obj: Object) -> Pose: + self.check_object_exists_and_issue_warning_if_not(obj) return self._get_body_pose(obj.name) def _get_body_pose(self, body_name: str) -> Pose: @@ -214,11 +217,14 @@ def _get_body_pose(self, body_name: str) -> Pose: return Pose(self.receive_data[1:4], self.receive_data[4:]) def reset_object_base_pose(self, obj: Object, pose: Pose): + self.check_object_exists_and_issue_warning_if_not(obj) self._reset_body_pose(obj.name, pose) - def _reset_body_pose(self, body_name: str, pose: Pose): + def _reset_body_pose(self, body_name: str, pose: Pose) -> None: """ Reset the pose of a body in the simulator. + param body_name: The name of the body. + param pose: The pose of the body. """ self._init_setter() self.request_meta_data["send"][body_name] = ["position", "quaternion"] @@ -226,6 +232,15 @@ def _reset_body_pose(self, body_name: str, pose: Pose): self.send_data = [time() - self.time_start, *pose.position_as_list(), *pose.orientation_as_list()] self.send_and_receive_data() + def get_all_objects_data_from_server(self) -> Dict[str, Dict]: + """ + Get all objects data from the multiverse server. + """ + self._init_getter() + self.request_meta_data["receive"][""] = [""] + self.send_and_receive_meta_data() + return self.response_meta_data["receive"] + def disconnect_from_physics_server(self) -> None: self.stop() @@ -251,10 +266,13 @@ def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") def get_object_contact_points(self, obj: Object) -> List: + self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_object_contact_points is not implemented in Multiverse") return [] def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + self.check_object_exists_and_issue_warning_if_not(obj1) + self.check_object_exists_and_issue_warning_if_not(obj2) logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") return [] @@ -282,21 +300,26 @@ def restore_physics_simulator_state(self, state_id: int) -> None: raise NotImplementedError def set_link_color(self, link: Link, rgba_color: Color): + self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("set_link_color is not implemented in Multiverse") def get_link_color(self, link: Link) -> Color: + self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("get_link_color is not implemented in Multiverse") return Color() def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: + self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_colors_of_object_links is not implemented in Multiverse") return {} def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: + self.check_object_exists_and_issue_warning_if_not(obj) logging.error("get_object_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: + self.check_object_exists_and_issue_warning_if_not(link.object) logging.error("get_link_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ab04384ae..26ac24306 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import time import unittest +from unittest import skip from typing_extensions import Optional @@ -28,16 +29,15 @@ def setUpClass(cls): cls.multiverse = Multiverse(simulation="pycram_test", client_addr=SocketAddress(port="5481"), is_prospection=True) - # cls.big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", - # pose=Pose([2, 2, 1], [0, 0, 0, 1])) + cls.big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", + pose=Pose([2, 2, 1], [0, 0, 0, 1])) @classmethod def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() def tearDown(self): - # self.multiverse.reset_world() - pass + self.multiverse.reset_world() def test_reset_world(self): self.big_bowl.set_position([1, 1, 0]) @@ -55,16 +55,11 @@ def test_spawn_object(self): def test_remove_object(self): milk = self.spawn_milk() milk.remove() - milk.set_position([1, 1, 1]) def test_check_object_exists(self): - self.multiverse.request_meta_data["send"] = {} - self.multiverse.request_meta_data["meta_data"]["world_name"] = "" - self.multiverse.request_meta_data["meta_data"]["simulation_name"] = self.multiverse._meta_data.simulation_name - self.multiverse.request_meta_data["receive"] = {} - print(self.multiverse.request_meta_data) - self.multiverse.send_and_receive_meta_data() - print(self.multiverse.response_meta_data) + milk = self.spawn_milk() + data = self.multiverse.get_all_objects_data_from_server() + self.assertTrue(milk.name in data) def test_set_position(self): milk = self.spawn_milk() @@ -81,12 +76,14 @@ def test_update_position(self): for i, v in enumerate([0, 0, 2]): self.assertAlmostEqual(milk_position[i], v) + @skip("Not implemented") def test_get_joint_position(self): # self.wooden_log.remove() self.spawn_robot() # joint_position = self.robot.get_joint_position("joint1") # self.assertAlmostEqual(joint_position, 0.0) + @skip("Not implemented") def test_set_joint_position(self): self.spawn_robot() joint_position = self.robot.get_joint_position("shoulder_pan_joint") @@ -95,7 +92,9 @@ def test_set_joint_position(self): joint_position = self.robot.get_joint_position("shoulder_pan_joint") self.assertAlmostEqual(joint_position, -1.0) + @skip("Not implemented") def test_set_robot_position(self): + robot = self.spawn_robot() self.robot.set_position([0, 0, 1]) self.assertEqual(self.robot.get_position_as_list(), [0, 0, 1]) From 204d3cd05d8d0ab4adae87d572d00d881f6ec1e5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 28 Mar 2024 15:06:42 +0100 Subject: [PATCH 007/189] [AbstractWorld] Tests are running. --- src/pycram/world_concepts/world_object.py | 3 ++ src/pycram/worlds/multiverse.py | 16 ++++++++-- test/test_multiverse.py | 36 ++++++++++++++--------- 3 files changed, 38 insertions(+), 17 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 298baa5f4..bfb8ad864 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -175,6 +175,9 @@ def _init_joints(self): self.joints = {} for joint_name, joint_id in self.joint_name_to_id.items(): joint_description = self.description.get_joint_by_name(joint_name) + if joint_description.type not in [JointType.PRISMATIC, JointType.REVOLUTE]: + rospy.logwarn(f"Joint type {joint_description.type} is not supported.") + continue self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) def _add_to_world_sync_obj_queue(self) -> None: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5d50dc78c..9b5669fa1 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -2,6 +2,7 @@ import os from time import time +import rospy from typing_extensions import List, Dict, Optional from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color @@ -160,7 +161,9 @@ def _init_spawn_object(self, name: str) -> None: self.send_and_receive_meta_data() def get_object_joint_names(self, obj: Object) -> List[str]: - return [joint.name for joint in obj.description.joints] + + return [joint.name for joint in obj.description.joints + if joint.type in [JointType.REVOLUTE, JointType.PRISMATIC]] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] @@ -192,10 +195,10 @@ def set_simulation_in_request_meta_data(self): def reset_joint_position(self, joint: Joint, joint_position: float) -> None: self._init_setter() - self.send_and_receive_meta_data() attribute = self.get_joint_position_name(joint) self.request_meta_data["send"][joint.name] = [attribute] - self.send_data = [time(), joint_position] + self.send_and_receive_meta_data() + self.send_data = [time() - self.time_start, joint_position] self.send_and_receive_data() def get_link_pose(self, link: Link) -> Pose: @@ -210,6 +213,7 @@ def _get_body_pose(self, body_name: str) -> Pose: self._init_getter() self.request_meta_data["receive"][body_name] = ["position", "quaternion"] self.send_and_receive_meta_data() + self.send_data = [time() - self.time_start] self.send_and_receive_data() if len(self.receive_data) != 8: logging.error(f"Invalid body pose data: {self.receive_data}") @@ -220,6 +224,12 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) self._reset_body_pose(obj.name, pose) + def multiverse_reset_world(self): + self._init_setter() + self.send_and_receive_meta_data() + self.send_data = [0] + self.send_and_receive_data() + def _reset_body_pose(self, body_name: str, pose: Pose) -> None: """ Reset the pose of a body in the simulator. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 26ac24306..e4208844e 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -38,6 +38,8 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.reset_world() + # self.multiverse.multiverse_reset_world() + pass def test_reset_world(self): self.big_bowl.set_position([1, 1, 0]) @@ -76,21 +78,27 @@ def test_update_position(self): for i, v in enumerate([0, 0, 2]): self.assertAlmostEqual(milk_position[i], v) - @skip("Not implemented") - def test_get_joint_position(self): - # self.wooden_log.remove() - self.spawn_robot() - # joint_position = self.robot.get_joint_position("joint1") - # self.assertAlmostEqual(joint_position, 0.0) - - @skip("Not implemented") + # @skip("Not implemented") def test_set_joint_position(self): - self.spawn_robot() - joint_position = self.robot.get_joint_position("shoulder_pan_joint") - self.robot.set_joint_position("shoulder_pan_joint", joint_position - 1.0) - self.robot.joints["shoulder_pan_joint"]._update_position() - joint_position = self.robot.get_joint_position("shoulder_pan_joint") - self.assertAlmostEqual(joint_position, -1.0) + robot = self.spawn_robot() + original_joint_position = robot.get_joint_position("joint1") + step = 1.57 + i = 0 + while True: + robot.set_joint_position("joint1", original_joint_position - step*i) + robot.joints["joint1"]._update_position() + joint_position = robot.get_joint_position("joint1") + if joint_position <= original_joint_position-1.57: + break + i += 1 + # time.sleep(0.1) + self.assertAlmostEqual(joint_position, original_joint_position-1.57) + + def test_destroy_robot(self): + robot = self.spawn_robot() + self.assertTrue(robot in self.multiverse.objects) + robot.remove() + self.assertTrue(robot not in self.multiverse.objects) @skip("Not implemented") def test_set_robot_position(self): From a4be93a4e9d02235e3e8a0f74ca286268f88d912 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 28 Mar 2024 15:26:28 +0100 Subject: [PATCH 008/189] [AbstractWorld] Tests are running. --- src/pycram/world_concepts/world_object.py | 5 +-- test/test_multiverse.py | 42 +++++++++++++++++------ 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index bfb8ad864..7ced7bad4 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -81,7 +81,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, + f"{self.name}_{self.id}") if robot_description is not None: - if self.description.name == robot_description.name: + if self.description.name == robot_description.name or self.obj_type == ObjectType.ROBOT: self.world.set_robot_if_not_set(self) self._init_joint_name_and_id_map() @@ -175,9 +175,6 @@ def _init_joints(self): self.joints = {} for joint_name, joint_id in self.joint_name_to_id.items(): joint_description = self.description.get_joint_by_name(joint_name) - if joint_description.type not in [JointType.PRISMATIC, JointType.REVOLUTE]: - rospy.logwarn(f"Joint type {joint_description.type} is not supported.") - continue self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) def _add_to_world_sync_obj_queue(self) -> None: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index e4208844e..d16c26e21 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -3,6 +3,7 @@ import unittest from unittest import skip +import psutil from typing_extensions import Optional from pycram.datastructures.enums import ObjectType @@ -16,13 +17,23 @@ except ImportError: multiverse_installed = False +processes = psutil.process_iter() +process_names = [p.name() for p in processes] +multiverse_running = True +mujoco_running = True +if 'multiverse_server' not in process_names: + multiverse_running = False +if 'mujoco' not in process_names: + mujoco_running = False + @unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") +@unittest.skipIf(not multiverse_running, "Multiverse server is not running.") +@unittest.skipIf(not mujoco_running, "Mujoco is not running.") # @unittest.skip("Needs Multiverse server and simulation to be running") class MultiversePyCRAMTestCase(unittest.TestCase): multiverse: Multiverse big_bowl: Optional[Object] = None - robot: Optional[Object] = None @classmethod def setUpClass(cls): @@ -42,8 +53,11 @@ def tearDown(self): pass def test_reset_world(self): - self.big_bowl.set_position([1, 1, 0]) - self.assertAlmostEqual(self.big_bowl.get_position_as_list(), [1, 1, 0]) + set_position = [1, 1, 0] + self.big_bowl.set_position(set_position) + bowl_position = self.big_bowl.get_position_as_list() + for i in range(3): + self.assertAlmostEqual(bowl_position[i], set_position[i]) self.multiverse.reset_world() big_bowl_pose = self.big_bowl.get_pose() self.assertAlmostEqual(big_bowl_pose, self.big_bowl.original_pose) @@ -78,7 +92,6 @@ def test_update_position(self): for i, v in enumerate([0, 0, 2]): self.assertAlmostEqual(milk_position[i], v) - # @skip("Not implemented") def test_set_joint_position(self): robot = self.spawn_robot() original_joint_position = robot.get_joint_position("joint1") @@ -94,17 +107,24 @@ def test_set_joint_position(self): # time.sleep(0.1) self.assertAlmostEqual(joint_position, original_joint_position-1.57) - def test_destroy_robot(self): + def test_spawn_robot(self): robot = self.spawn_robot() + self.assertIsInstance(robot, Object) self.assertTrue(robot in self.multiverse.objects) - robot.remove() - self.assertTrue(robot not in self.multiverse.objects) + self.assertTrue(self.multiverse.robot.name == robot.name) + + def test_destroy_robot(self): + if self.multiverse.robot is None: + self.spawn_robot() + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + self.multiverse.robot.remove() + self.assertTrue(self.multiverse.robot not in self.multiverse.objects) - @skip("Not implemented") def test_set_robot_position(self): - robot = self.spawn_robot() - self.robot.set_position([0, 0, 1]) - self.assertEqual(self.robot.get_position_as_list(), [0, 0, 1]) + if self.multiverse.robot is None: + self.spawn_robot() + self.multiverse.robot.set_position([0, 0, 1]) + self.assertEqual(self.multiverse.robot.get_position_as_list(), [0, 0, 1]) @staticmethod def spawn_milk() -> Object: From 6acc6f3d89e264f82911f3b491fe3ce9ec2f08ee Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 10 May 2024 10:44:04 +0200 Subject: [PATCH 009/189] [Multiverse] Adding API callbacks to multiverse. --- src/pycram/worlds/multiverse.py | 46 ++++++++++++++++++++++++++++++--- test/test_multiverse.py | 14 ++++++++++ 2 files changed, 56 insertions(+), 4 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 9b5669fa1..8ac2d835c 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -2,7 +2,6 @@ import os from time import time -import rospy from typing_extensions import List, Dict, Optional from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color @@ -111,6 +110,8 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.simulation: str = simulation self._make_sure_multiverse_resources_are_added() self.last_object_id: int = -1 + self.last_constraint_id: int = -1 + self.constraints: Dict[int, Constraint] = {} self.object_name_to_id: Dict[str, int] = {} self.object_id_to_name: Dict[int, str] = {} self.time_start = time() @@ -266,11 +267,48 @@ def remove_object_from_simulator(self, obj: Object) -> None: self.send_and_receive_data() def add_constraint(self, constraint: Constraint) -> int: - logging.warning("add_constraint is not implemented in Multiverse") - return 0 + if constraint.type != JointType.FIXED: + logging.error("Only fixed constraints are supported in Multiverse") + raise ValueError + constraint_id = self.last_constraint_id + 1 + self._add_api_request("attach", constraint.parent_link.name, constraint.child_link.name) + self._send_api_request() + self.constraints[constraint_id] = constraint + return constraint_id def remove_constraint(self, constraint_id) -> None: - logging.warning("remove_constraint is not implemented in Multiverse") + constraint = self.constraints.pop(constraint_id) + self._add_api_request("detach", constraint.parent_link.name, constraint.child_link.name) + self._send_api_request() + + def _init_api_callback(self): + """ + Initialize the API callback in the request metadata. + """ + self.request_meta_data["send"] = {} + self.request_meta_data["receive"] = {} + self.request_meta_data["api_callbacks"] = {self.simulation: []} + self.set_simulation_in_request_meta_data() + + def _add_api_request(self, api_name: str, *params): + """ + Add an API request to the request metadata. + param api_name: The name of the API. + param params: The parameters of the API. + """ + if "api_callbacks" not in self.request_meta_data: + self._init_api_callback() + self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) + + def _send_api_request(self): + """ + Send the API request to the server. + """ + if "api_callbacks" not in self.request_meta_data: + logging.error("No API request to send") + raise ValueError + self.send_and_receive_meta_data() + self.request_meta_data.pop("api_callbacks") def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index d16c26e21..648313d43 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -126,6 +126,20 @@ def test_set_robot_position(self): self.multiverse.robot.set_position([0, 0, 1]) self.assertEqual(self.multiverse.robot.get_position_as_list(), [0, 0, 1]) + def test_attach(self): + if self.multiverse.robot is None: + self.spawn_robot() + self.multiverse.robot.attach(self.big_bowl) + self.assertTrue(self.big_bowl in self.multiverse.robot.attachments) + bowl_position = self.big_bowl.get_position_as_list() + robot_position = self.multiverse.robot.get_position_as_list() + robot_position[0] += 1 + self.multiverse.robot.set_position(robot_position) + new_bowl_position = self.big_bowl.get_position_as_list() + estimated_bowl_position = bowl_position + estimated_bowl_position[0] += 1 + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + @staticmethod def spawn_milk() -> Object: return Object("milk_box", ObjectType.MILK, "milk_box.urdf", From fedaad9dca34da133368c048910d4612dfa8a343 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 10 May 2024 17:11:10 +0200 Subject: [PATCH 010/189] [Multiverse] Attach seems to work, but there's some problem with setting robot position. --- src/pycram/description.py | 6 ++ src/pycram/local_transformer.py | 3 +- src/pycram/object_descriptors/urdf.py | 12 ++++ src/pycram/world.py | 5 +- src/pycram/world_concepts/world_object.py | 68 ++++++++++++++--------- src/pycram/worlds/multiverse.py | 37 +++++++++--- test/test_multiverse.py | 38 +++++++------ 7 files changed, 118 insertions(+), 51 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index a4accfc15..823463cc5 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -725,6 +725,12 @@ def get_root(self) -> str: """ pass + def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + """ + raise NotImplementedError + @abstractmethod def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index e045d0148..108864279 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -77,7 +77,8 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Union[Pose, None]: copy_pose.header.stamp = rospy.Time(0) if not self.canTransform(target_frame, pose.frame, rospy.Time(0)): rospy.logerr( - f"Can not transform pose: \n {pose}\n to frame: {target_frame}.\n Maybe try calling 'update_transforms_for_object'") + f"Can not transform pose: \n {pose}\n to frame: {target_frame}." + f"\n Maybe try calling 'update_transforms_for_object'") return new_pose = super().transformPose(target_frame, copy_pose) diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index d21490d31..73a0f4a33 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -264,6 +264,18 @@ def get_root(self) -> str: """ return self.parsed_description.get_root() + def get_tip(self) -> str: + link = self.get_root() + while link in self.parsed_description.child_map: + children = self.parsed_description.child_map[link] + if len(children) > 1: + # Multiple children, can't decide which one to take (e.g. fingers of a hand) + break + else: + child = children[0][1] + link = child + return link + def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ :return: the chain of links from 'start_link_name' to 'end_link_name'. diff --git a/src/pycram/world.py b/src/pycram/world.py index a1d64e65a..6d739cc05 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -170,8 +170,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.objects: List[Object] = [] # List of all Objects in the World - - self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" @@ -179,6 +177,9 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_events() + self.set_attached_objects_poses = True + self.handle_spawning = True + self._current_state: Optional[WorldState] = None @abstractmethod diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index dcaac7170..879382e0a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -2,23 +2,22 @@ import logging import os -from typing import Iterable import numpy as np import rospy from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union +from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, + AxisAlignedBoundingBox, VisualShape) +from ..datastructures.enums import ObjectType, JointType +from ..datastructures.pose import Pose, Transform from ..description import ObjectDescription, LinkDescription +from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_descriptions import robot_description from ..world import WorldEntity, World from ..world_concepts.constraints import Attachment -from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape) -from ..datastructures.enums import ObjectType, JointType -from ..local_transformer import LocalTransformer -from ..datastructures.pose import Pose, Transform Link = ObjectDescription.Link @@ -48,7 +47,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param name: The name of the object :param obj_type: The type of the object as an ObjectType enum. - :param path: The path to the source file, if only a filename is provided then the resources directories will be searched. + :param path: The path to the source file, if only a filename is provided then the resources directories will be + searched. :param description: The ObjectDescription of the object, this contains the joints and links of the object. :param pose: The pose at which the Object should be spawned :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used. @@ -103,7 +103,7 @@ def pose(self): def pose(self, pose: Pose): self.set_pose(pose) - def _load_object_and_get_id(self, path: Optional[str] = None, + def _load_object_and_get_id(self, path: str, ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be @@ -114,23 +114,21 @@ def _load_object_and_get_id(self, path: Optional[str] = None, This new file will have resolved mesh file paths, meaning there will be no references to ROS packges instead there will be absolute file paths. - :param path: The path to the description file, if None then no file will be loaded, this is useful when the PyCRAM is not responsible for loading the file but another system is. + :param path: The path to the description file. :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path of the file that was loaded. """ - if path is not None: - try: - self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) - except FileNotFoundError as e: - logging.error("Could not generate description from file.") - raise e - - else: - self.path = self.name + try: + self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + except FileNotFoundError as e: + logging.error("Could not generate description from file.") + raise e try: - obj_id = self.world.load_object_and_get_id(self, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) + if not self.world.handle_spawning: + path = self.name + obj_id = self.world.load_object_and_get_id(path, Pose(self.get_position_as_list(), + self.get_orientation_as_list())) return obj_id, self.path except Exception as e: @@ -154,7 +152,7 @@ def _init_link_name_and_id_map(self) -> None: """ n_links = len(self.link_names) self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) - self.link_name_to_id[self.description.get_root()] = -1 + self.link_name_to_id[self.root_link_name] = -1 self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) def _init_links_and_update_transforms(self) -> None: @@ -165,7 +163,7 @@ def _init_links_and_update_transforms(self) -> None: self.links = {} for link_name, link_id in self.link_name_to_id.items(): link_description = self.description.get_link_by_name(link_name) - if link_name == self.description.get_root(): + if link_name == self.root_link_name: self.links[link_name] = self.description.RootLink(self) else: self.links[link_name] = self.description.Link(link_id, link_description, self) @@ -648,6 +646,8 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ + if not self.world.set_attached_objects_poses: + return if already_moved_objects is None: already_moved_objects = [] @@ -743,7 +743,16 @@ def root_link(self) -> ObjectDescription.Link: :return: The root link of this object. """ - return self.links[self.description.get_root()] + return self.links[self.root_link_name] + + @property + def tip_link(self) -> ObjectDescription.Link: + """ + Returns the tip link of this object. + + :return: The tip link of this object. + """ + return self.links[self.tip_link_name] @property def root_link_name(self) -> str: @@ -754,13 +763,22 @@ def root_link_name(self) -> str: """ return self.description.get_root() + @property + def tip_link_name(self) -> str: + """ + Returns the name of the tip link of this object. + + :return: The name of the tip link of this object. + """ + return self.description.get_tip() + def get_root_link_id(self) -> int: """ Returns the unique id of the root link of this object. :return: The unique id of the root link of this object. """ - return self.get_link_id(self.description.get_root()) + return self.get_link_id(self.root_link_name) def get_link_id(self, link_name: str) -> int: """ @@ -885,7 +903,7 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: :param joint_type: Joint type that should be searched for :return: Name of the first joint which has the given type """ - chain = self.description.get_chain(self.description.get_root(), link_name) + chain = self.description.get_chain(self.root_link_name, link_name) reversed_chain = reversed(chain) container_joint = None for element in reversed_chain: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 8ac2d835c..10cdb2e0e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -109,6 +109,8 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT World.__init__(self, mode, is_prospection, simulation_frequency) self.simulation: str = simulation self._make_sure_multiverse_resources_are_added() + self.set_attached_objects_poses = False + self.handle_spawning = False self.last_object_id: int = -1 self.last_constraint_id: int = -1 self.constraints: Dict[int, Constraint] = {} @@ -117,6 +119,9 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.time_start = time() self.run() + def _init_world(self, mode: WorldMode): + pass + def _make_sure_multiverse_resources_are_added(self): """ Add the multiverse resources to the pycram world resources. @@ -134,22 +139,23 @@ def get_joint_position_name(self, joint: Joint) -> str: return "joint_rvalue" return self._joint_type_to_position_name[joint.type] - def load_object_and_get_id(self, obj: Object, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, name: Optional[str] = None, + pose: Optional[Pose] = None) -> int: """ Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as the name of the object in the description file. - param obj: The object to be loaded. + param name: The name of the object to be loaded. param pose: The pose of the object. """ if pose is None: pose = Pose() - self._reset_body_pose(obj.name, pose) + self._reset_body_pose(name, pose) self.last_object_id += 1 - self.object_name_to_id[obj.name] = self.last_object_id - self.object_id_to_name[self.last_object_id] = obj.name + self.object_name_to_id[name] = self.last_object_id + self.object_id_to_name[self.last_object_id] = name return self.last_object_id @@ -271,16 +277,29 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError constraint_id = self.last_constraint_id + 1 - self._add_api_request("attach", constraint.parent_link.name, constraint.child_link.name) + self._add_api_request("attach", constraint.parent_link.name, + constraint.child_link.name, self._get_attachment_pose_as_string(constraint)) self._send_api_request() self.constraints[constraint_id] = constraint return constraint_id def remove_constraint(self, constraint_id) -> None: constraint = self.constraints.pop(constraint_id) - self._add_api_request("detach", constraint.parent_link.name, constraint.child_link.name) + self._add_api_request("detach", constraint.parent_link.name, + constraint.child_link.name) self._send_api_request() + def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: + self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) + self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) + pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) + return self._pose_to_string(pose) + + @staticmethod + def _pose_to_string(pose: Pose) -> str: + return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ + f"{pose.orientation.y} {pose.orientation.z}" + def _init_api_callback(self): """ Initialize the API callback in the request metadata. @@ -376,3 +395,7 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") + + def check_object_exists_and_issue_warning_if_not(self, object): + if object not in self.objects: + logging.warning(f"Object {object.name} does not exist in the simulator") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 648313d43..8e5a2b41d 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -8,6 +8,7 @@ from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose +from pycram.world import UseProspectionWorld from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse @@ -123,22 +124,25 @@ def test_destroy_robot(self): def test_set_robot_position(self): if self.multiverse.robot is None: self.spawn_robot() - self.multiverse.robot.set_position([0, 0, 1]) - self.assertEqual(self.multiverse.robot.get_position_as_list(), [0, 0, 1]) + new_position = [1, 1, 0] + self.multiverse.robot.set_position(new_position) + self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) def test_attach(self): - if self.multiverse.robot is None: - self.spawn_robot() - self.multiverse.robot.attach(self.big_bowl) - self.assertTrue(self.big_bowl in self.multiverse.robot.attachments) - bowl_position = self.big_bowl.get_position_as_list() - robot_position = self.multiverse.robot.get_position_as_list() - robot_position[0] += 1 - self.multiverse.robot.set_position(robot_position) - new_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = bowl_position - estimated_bowl_position[0] += 1 - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + with UseProspectionWorld(): + if self.multiverse.robot is None: + robot = self.spawn_robot() + robot.attach(self.big_bowl) + self.assertTrue(self.big_bowl in robot.attachments) + bowl_position = self.big_bowl.get_position_as_list() + robot_position = robot.get_position_as_list() + robot_position[0] += 1 + time.sleep(2) + robot.set_position(robot_position) + new_bowl_position = self.big_bowl.get_position_as_list() + estimated_bowl_position = bowl_position + estimated_bowl_position[0] += 1 + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) @staticmethod def spawn_milk() -> Object: @@ -147,5 +151,7 @@ def spawn_milk() -> Object: @staticmethod def spawn_robot() -> Object: - return Object("panda", ObjectType.ROBOT, "panda.urdf", - pose=Pose([0, 0, 3], [0, 0, 0, 1])) + robot = Object("panda", ObjectType.ROBOT, "panda.urdf", + pose=Pose([1, 0, 3], [0, 0, 0, 1])) + time.sleep(2) + return robot From 2664843deb409ab16a5c55c4a998890bc7fe1562 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 10 May 2024 19:21:15 +0200 Subject: [PATCH 011/189] [Multiverse] Attach is working. --- src/pycram/worlds/multiverse.py | 44 ++++++++++++++++++++++++++++----- test/test_multiverse.py | 30 ++++++++++++++-------- 2 files changed, 58 insertions(+), 16 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 10cdb2e0e..7cbac7b90 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -113,7 +113,7 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.handle_spawning = False self.last_object_id: int = -1 self.last_constraint_id: int = -1 - self.constraints: Dict[int, Constraint] = {} + self.constraints: Dict[int, Dict[str, str]] = {} self.object_name_to_id: Dict[str, int] = {} self.object_id_to_name: Dict[int, str] = {} self.time_start = time() @@ -277,26 +277,56 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError constraint_id = self.last_constraint_id + 1 - self._add_api_request("attach", constraint.parent_link.name, - constraint.child_link.name, self._get_attachment_pose_as_string(constraint)) + parent_link_name = self.get_link_name_from_constraint_link(constraint.parent_link) + child_link_name = self.get_link_name_from_constraint_link(constraint.child_link) + self._add_api_request("attach", parent_link_name, + child_link_name, self._get_attachment_pose_as_string(constraint)) self._send_api_request() - self.constraints[constraint_id] = constraint + self.constraints[constraint_id] = {'parent_link': parent_link_name, + 'child_link': child_link_name} return constraint_id def remove_constraint(self, constraint_id) -> None: constraint = self.constraints.pop(constraint_id) - self._add_api_request("detach", constraint.parent_link.name, - constraint.child_link.name) + self._add_api_request("detach", constraint['parent_link'], constraint['child_link']) self._send_api_request() + def get_link_name_from_constraint_link(self, link: Link) -> str: + """ + Get the link name from the constraint link, if the link belongs to a one link object, return the object name. + param link: The link. + return: The link name. + """ + return link.name if not self.is_one_link_object(link.object) else link.object.name + + @staticmethod + def is_one_link_object(obj: Object) -> bool: + """ + Check if the object has only one link. + param obj: The object. + return: True if the object has only one link, False otherwise. + """ + return len(obj.links) == 1 + def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: + """ + Get the attachment pose as a string. + param constraint: The constraint. + return: The attachment pose as a string. + """ self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) + print(pose) return self._pose_to_string(pose) @staticmethod def _pose_to_string(pose: Pose) -> str: + """ + Convert the pose to a string. + param pose: The pose. + return: The pose as a string. + """ return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ f"{pose.orientation.y} {pose.orientation.z}" @@ -328,6 +358,8 @@ def _send_api_request(self): raise ValueError self.send_and_receive_meta_data() self.request_meta_data.pop("api_callbacks") + print(self.response_meta_data) + def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 8e5a2b41d..a5e67cfee 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,10 +1,9 @@ #!/usr/bin/env python3 import time import unittest -from unittest import skip import psutil -from typing_extensions import Optional +from typing_extensions import Optional, List from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose @@ -99,14 +98,14 @@ def test_set_joint_position(self): step = 1.57 i = 0 while True: - robot.set_joint_position("joint1", original_joint_position - step*i) + robot.set_joint_position("joint1", original_joint_position - step * i) robot.joints["joint1"]._update_position() joint_position = robot.get_joint_position("joint1") - if joint_position <= original_joint_position-1.57: + if joint_position <= original_joint_position - 1.57: break i += 1 # time.sleep(0.1) - self.assertAlmostEqual(joint_position, original_joint_position-1.57) + self.assertAlmostEqual(joint_position, original_joint_position - 1.57) def test_spawn_robot(self): robot = self.spawn_robot() @@ -121,6 +120,15 @@ def test_destroy_robot(self): self.multiverse.robot.remove() self.assertTrue(self.multiverse.robot not in self.multiverse.objects) + @unittest.skip("Not implemented feature yet.") + def test_respawmn_robot(self): + self.spawn_robot() + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + self.multiverse.robot.remove() + self.assertTrue(self.multiverse.robot not in self.multiverse.objects) + self.spawn_robot(position=[0, 0, 1]) + self.assertTrue(self.multiverse.robot in self.multiverse.objects) + def test_set_robot_position(self): if self.multiverse.robot is None: self.spawn_robot() @@ -135,13 +143,13 @@ def test_attach(self): robot.attach(self.big_bowl) self.assertTrue(self.big_bowl in robot.attachments) bowl_position = self.big_bowl.get_position_as_list() + robot.update_pose() robot_position = robot.get_position_as_list() - robot_position[0] += 1 - time.sleep(2) + robot_position[2] += 3 robot.set_position(robot_position) new_bowl_position = self.big_bowl.get_position_as_list() estimated_bowl_position = bowl_position - estimated_bowl_position[0] += 1 + estimated_bowl_position[2] += 3 self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) @staticmethod @@ -150,8 +158,10 @@ def spawn_milk() -> Object: pose=Pose([0, 0, 2], [0, 0, 0, 1])) @staticmethod - def spawn_robot() -> Object: + def spawn_robot(position: Optional[List[float]] = None) -> Object: + if position is None: + position = [0, 0, 0] robot = Object("panda", ObjectType.ROBOT, "panda.urdf", - pose=Pose([1, 0, 3], [0, 0, 0, 1])) + pose=Pose(position, [0, 0, 0, 1])) time.sleep(2) return robot From 7400c6c77bc4c8ef4eebb4fc36c71ac17f676b3d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 10 May 2024 19:44:45 +0200 Subject: [PATCH 012/189] [Multiverse] Attach test is working. --- src/pycram/worlds/multiverse.py | 3 --- test/test_multiverse.py | 20 ++++++++++++++++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7cbac7b90..24dd91c81 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -317,7 +317,6 @@ def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) - print(pose) return self._pose_to_string(pose) @staticmethod @@ -358,8 +357,6 @@ def _send_api_request(self): raise ValueError self.send_and_receive_meta_data() self.request_meta_data.pop("api_callbacks") - print(self.response_meta_data) - def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index a5e67cfee..76f9c50b1 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -136,7 +136,21 @@ def test_set_robot_position(self): self.multiverse.robot.set_position(new_position) self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) - def test_attach(self): + def test_attach_object(self): + milk = self.spawn_milk() + milk.attach(self.big_bowl) + self.assertTrue(self.big_bowl in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 3 + big_bowl_position = self.big_bowl.get_position_as_list() + estimated_bowl_position = big_bowl_position + estimated_bowl_position[0] += 3 + milk.set_position(milk_position) + new_bowl_position = self.big_bowl.get_position_as_list() + self.assertAlmostEqual(new_bowl_position[2], estimated_bowl_position[2]) + + @unittest.skip("Needs mobile base robot") + def test_attach_with_robot(self): with UseProspectionWorld(): if self.multiverse.robot is None: robot = self.spawn_robot() @@ -154,8 +168,10 @@ def test_attach(self): @staticmethod def spawn_milk() -> Object: - return Object("milk_box", ObjectType.MILK, "milk_box.urdf", + milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", pose=Pose([0, 0, 2], [0, 0, 0, 1])) + time.sleep(2) + return milk @staticmethod def spawn_robot(position: Optional[List[float]] = None) -> Object: From cbc6e1b71dfeda79390c53bf525e2d78774f0461 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 23 May 2024 12:56:08 +0200 Subject: [PATCH 013/189] [Multiverse] Added a flag that enables update of poses when getting. Corrected path variable assignment when loading urdf. --- src/pycram/description.py | 6 +++++- src/pycram/world.py | 1 + src/pycram/world_concepts/world_object.py | 9 +++++---- src/pycram/worlds/multiverse.py | 1 + test/test_multiverse.py | 8 ++++---- 5 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 823463cc5..40bd40b65 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -339,6 +339,8 @@ def pose(self) -> Pose: :return: A Pose object containing the pose of the link relative to the world frame. """ + if self.world.update_poses_on_get: + self._update_pose() return self._current_pose @property @@ -415,7 +417,7 @@ def tf_frame(self) -> str: return self.object.tf_frame def _update_pose(self) -> None: - self._current_pose = self.object.get_pose() + self._current_pose = self.world.get_object_pose(self.object) def __copy__(self): return RootLink(self.object) @@ -475,6 +477,8 @@ def child_link(self) -> Link: @property def position(self) -> float: + if self.world.update_poses_on_get: + self._update_position() return self._current_position def reset_position(self, position: float) -> None: diff --git a/src/pycram/world.py b/src/pycram/world.py index 6d739cc05..2348183fd 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -179,6 +179,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.set_attached_objects_poses = True self.handle_spawning = True + self.update_poses_on_get = False self._current_state: Optional[WorldState] = None diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 879382e0a..9bc6ad68c 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -125,10 +125,8 @@ def _load_object_and_get_id(self, path: str, raise e try: - if not self.world.handle_spawning: - path = self.name - obj_id = self.world.load_object_and_get_id(path, Pose(self.get_position_as_list(), - self.get_orientation_as_list())) + path = self.path if self.world.handle_spawning else self.name + obj_id = self.world.load_object_and_get_id(path, self._current_pose) return obj_id, self.path except Exception as e: @@ -474,6 +472,8 @@ def get_pose(self) -> Pose: :return: The current pose of this object """ + if self.world.update_poses_on_get: + self.update_pose() return self._current_pose def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: @@ -502,6 +502,7 @@ def update_pose(self): Updates the current pose of this object from the world, and updates the poses of all links. """ self._current_pose = self.world.get_object_pose(self) + # TODO: Probably not needed, need to test self._update_all_links_poses() self.update_link_transforms() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 24dd91c81..f450f57f6 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -111,6 +111,7 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self._make_sure_multiverse_resources_are_added() self.set_attached_objects_poses = False self.handle_spawning = False + self.update_poses_on_get = True self.last_object_id: int = -1 self.last_constraint_id: int = -1 self.constraints: Dict[int, Dict[str, str]] = {} diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 76f9c50b1..73ab8dc6c 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -141,13 +141,13 @@ def test_attach_object(self): milk.attach(self.big_bowl) self.assertTrue(self.big_bowl in milk.attachments) milk_position = milk.get_position_as_list() - milk_position[0] += 3 + milk_position[0] += 1 big_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = big_bowl_position - estimated_bowl_position[0] += 3 + estimated_bowl_position = big_bowl_position.copy() + estimated_bowl_position[0] += 1 milk.set_position(milk_position) new_bowl_position = self.big_bowl.get_position_as_list() - self.assertAlmostEqual(new_bowl_position[2], estimated_bowl_position[2]) + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) @unittest.skip("Needs mobile base robot") def test_attach_with_robot(self): From e2ef33b45fa68b093f420cc00a9251c3d8e696b7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 24 May 2024 19:52:34 +0200 Subject: [PATCH 014/189] [NEEMReplay] Added the ability to detect close points between objects. --- src/pycram/description.py | 3 +- src/pycram/world.py | 36 +++++++++++++++++++---- src/pycram/world_concepts/world_object.py | 19 ++++++++++++ src/pycram/worlds/bullet_world.py | 3 ++ 4 files changed, 54 insertions(+), 7 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 40bd40b65..c56285fd3 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -236,7 +236,8 @@ def remove_constraint_with_link(self, child_link: 'Link') -> None: """ self.world.remove_constraint(self.constraint_ids[child_link]) del self.constraint_ids[child_link] - del child_link.constraint_ids[self] + if self in child_link.constraint_ids.keys(): + del child_link.constraint_ids[self] @property def is_root(self) -> bool: diff --git a/src/pycram/world.py b/src/pycram/world.py index 2348183fd..760447d10 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -15,15 +15,15 @@ from typing_extensions import Union from .cache_manager import CacheManager +from .datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, State, WorldState) from .datastructures.enums import JointType, ObjectType, WorldMode -from .world_concepts.event import Event -from .local_transformer import LocalTransformer from .datastructures.pose import Pose, Transform +from .local_transformer import LocalTransformer from .world_concepts.constraints import Constraint -from .datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) +from .world_concepts.event import Event if TYPE_CHECKING: from .world_concepts.world_object import Object @@ -472,6 +472,30 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass + def get_object_closest_points(self, obj: Object, max_distance: float) -> List: + """ + Returns the closest points of this object with all other objects in the world. + + :param obj: The object. + :param max_distance: The maximum distance between the points. + :return: A list of the closest points. + """ + closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in + self.objects + if other_obj != obj] + return [point for points in closest_points for point in points if len(point) > 0] + + def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) -> List: + """ + Returns the closest points between two objects. + + :param object_a: The first object. + :param object_b: The second object. + :param max_distance: The maximum distance between the points. + :return: A list of the closest points. + """ + raise NotImplementedError + @abstractmethod def reset_joint_position(self, joint: Joint, joint_position: float) -> None: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9bc6ad68c..c67849070 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -950,6 +950,25 @@ def contact_points_simulated(self) -> List: self.world.restore_state(state_id) return contact_points + def closest_points(self, max_distance: float) -> List: + """ + Returns a list of closest points between this Object and other Objects. + + :param max_distance: The maximum distance between the closest points + :return: A list of closest points between this Object and other Objects + """ + return self.world.get_object_closest_points(self, max_distance) + + def closest_points_with_obj(self, other_object: Object, max_distance: float) -> List: + """ + Returns a list of closest points between this Object and another Object. + + :param other_object: The other object + :param max_distance: The maximum distance between the closest points + :return: A list of closest points between this Object and the other Object + """ + return self.world.get_closest_points_between_objects(self, other_object, max_distance) + def set_color(self, rgba_color: Color) -> None: """ Changes the color of this object, the color has to be given as a list diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index f1bbc00bb..b645900d7 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -138,6 +138,9 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> self.perform_collision_detection() return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.id) + def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> List: + return p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) + def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: str) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) From 8c8b5cd2b6ad9bf616affb1b495c0604b11cf336 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 31 May 2024 11:54:58 +0200 Subject: [PATCH 015/189] [Contact] Created a dataclass for ContactPoint and ClosestPoint instead of having them as lists. --- src/pycram/datastructures/dataclasses.py | 24 ++++++++++++++ src/pycram/world.py | 23 ++++++------- src/pycram/world_concepts/world_object.py | 6 ++-- src/pycram/world_reasoning.py | 13 ++++---- src/pycram/worlds/bullet_world.py | 39 +++++++++++++++++------ src/pycram/worlds/multiverse.py | 4 +-- 6 files changed, 77 insertions(+), 32 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d83bcd00f..d7f7c7d90 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -296,3 +296,27 @@ class ObjectState(State): class WorldState(State): simulator_state_id: int object_states: Dict[str, ObjectState] + + +@dataclass +class LateralFriction: + lateral_friction: float + lateral_friction_direction: List[float] + + +@dataclass +class ContactPoint: + link_a: Link + link_b: Link + position_on_object_a: Optional[List[float]] = None + position_on_object_b: Optional[List[float]] = None + normal_on_b: Optional[List[float]] = None # normal on object b pointing towards object a + distance: Optional[float] = None + normal_force: Optional[List[float]] = None # normal force applied during last step simulation + lateral_friction_1: Optional[LateralFriction] = None + lateral_friction_2: Optional[LateralFriction] = None + + +@dataclass +class ClosestPoint(ContactPoint): + pass diff --git a/src/pycram/world.py b/src/pycram/world.py index 760447d10..18885aac2 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -18,7 +18,7 @@ from .datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState) + ObjectState, State, WorldState, ContactPoint, ClosestPoint) from .datastructures.enums import JointType, ObjectType, WorldMode from .datastructures.pose import Pose, Transform from .local_transformer import LocalTransformer @@ -420,7 +420,7 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: self.step() for objects, callbacks in self.coll_callbacks.items(): contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1]) - if contact_points != (): + if len(contact_points) > 0: callbacks.on_collision_cb() elif callbacks.no_collision_cb is not None: callbacks.no_collision_cb() @@ -452,7 +452,7 @@ def perform_collision_detection(self) -> None: pass @abstractmethod - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: """ Returns a list of contact points of this Object with all other Objects. @@ -462,9 +462,9 @@ def get_object_contact_points(self, obj: Object) -> List: pass @abstractmethod - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List[ContactPoint]: """ - Returns a list of contact points between obj1 and obj2. + Returns a list of contact points between obj_a and obj_b. :param obj1: The first object. :param obj2: The second object. @@ -472,7 +472,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass - def get_object_closest_points(self, obj: Object, max_distance: float) -> List: + def get_object_closest_points(self, obj: Object, max_distance: float) -> List[ClosestPoint]: """ Returns the closest points of this object with all other objects in the world. @@ -480,12 +480,13 @@ def get_object_closest_points(self, obj: Object, max_distance: float) -> List: :param max_distance: The maximum distance between the points. :return: A list of the closest points. """ - closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in - self.objects - if other_obj != obj] - return [point for points in closest_points for point in points if len(point) > 0] + all_obj_closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in + self.objects + if other_obj != obj] + return [point for closest_points in all_obj_closest_points for point in closest_points] - def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) -> List: + def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \ + -> List[ClosestPoint]: """ Returns the closest points between two objects. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index c67849070..68cff1c1e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -9,7 +9,7 @@ from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape) + AxisAlignedBoundingBox, VisualShape, ClosestPoint) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..description import ObjectDescription, LinkDescription @@ -950,7 +950,7 @@ def contact_points_simulated(self) -> List: self.world.restore_state(state_id) return contact_points - def closest_points(self, max_distance: float) -> List: + def closest_points(self, max_distance: float) -> List[ClosestPoint]: """ Returns a list of closest points between this Object and other Objects. @@ -959,7 +959,7 @@ def closest_points(self, max_distance: float) -> List: """ return self.world.get_object_closest_points(self, max_distance) - def closest_points_with_obj(self, other_object: Object, max_distance: float) -> List: + def closest_points_with_obj(self, other_object: Object, max_distance: float) -> List[ClosestPoint]: """ Returns a list of closest points between this Object and another Object. diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 7d91fc3dd..ddc0e415e 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -6,7 +6,7 @@ from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .datastructures.pose import Pose, Transform from .robot_descriptions import robot_description -from .world_concepts.world_object import Object +from .world_concepts.world_object import Object, Link from .world import World, UseProspectionWorld @@ -35,10 +35,10 @@ def stable(obj: Object) -> bool: def contact( object1: Object, object2: Object, - return_links: bool = False) -> Union[bool, Tuple[bool, List]]: + return_links: bool = False) -> Union[bool, Tuple[bool, List[Tuple[Link, Link]]]]: """ Checks if two objects are in contact or not. If the links should be returned then the output will also contain a - list of tuples where the first element is the link name of 'object1' and the second element is the link name of + list of tuples where the first element is the link of 'object1' and the second element is the link of 'object2'. :param object1: The first object @@ -57,12 +57,11 @@ def contact( if return_links: contact_links = [] for point in con_points: - contact_links.append((prospection_obj1.get_link_by_id(point[3]), - prospection_obj2.get_link_by_id(point[4]))) - return con_points != (), contact_links + contact_links.append((point.link_a, point.link_b)) + return len(con_points) > 0, contact_links else: - return con_points != () + return len(con_points) > 0 def get_visible_objects( diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index b645900d7..25579eab1 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -11,12 +11,13 @@ from geometry_msgs.msg import Point from typing_extensions import List, Optional, Dict +from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ + ClosestPoint, LateralFriction, ContactPoint from ..datastructures.enums import ObjectType, WorldMode, JointType from ..datastructures.pose import Pose from ..object_descriptors.urdf import ObjectDescription from ..world import World from ..world_concepts.constraints import Constraint -from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape from ..world_concepts.world_object import Object Link = ObjectDescription.Link @@ -125,21 +126,40 @@ def get_object_number_of_links(self, obj: Object) -> int: def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: """ For a more detailed explanation of the returned list please look at: `PyBullet Doc `_ """ self.perform_collision_detection() - return p.getContactPoints(obj.id, physicsClientId=self.id) + points_list = p.getContactPoints(obj.id, physicsClientId=self.id) + return [ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> List[ContactPoint]: self.perform_collision_detection() - return p.getContactPoints(obj1.id, obj2.id, physicsClientId=self.id) + points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id) + return [ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] - def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> List: - return p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) + def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> List[ClosestPoint]: + points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) + return [ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] + + @staticmethod + def parse_points_list_to_args(point: List) -> Dict: + """ + Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the + ContactPoint class. + """ + return {"link_a": point[3], + "link_b": point[4], + "position_on_object_a": point[5], + "position_on_object_b": point[6], + "normal_on_b": point[7], + "distance": point[8], + "normal_force": point[9], + "lateral_friction_1": LateralFriction(point[10], point[11]), + "lateral_friction_2": LateralFriction(point[12], point[13])} def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: str) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) @@ -395,7 +415,7 @@ def run(self): width, height, dist = (p.getDebugVisualizerCamera()[0], p.getDebugVisualizerCamera()[1], p.getDebugVisualizerCamera()[10]) - #print("width: ", width, "height: ", height, "dist: ", dist) + # print("width: ", width, "height: ", height, "dist: ", dist) camera_target_position = p.getDebugVisualizerCamera(self.world.id)[11] # Get vectors used for movement on x,y,z Vector @@ -550,5 +570,6 @@ def run(self): cameraTargetPosition=camera_target_position, physicsClientId=self.world.id) if visible == 0: camera_target_position = (0.0, -50, 50) - p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1], physicsClientId=self.world.id) + p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1], + physicsClientId=self.world.id) time.sleep(1. / 80.) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index f450f57f6..b50356033 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,7 +4,7 @@ from typing_extensions import List, Dict, Optional -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPoint from ..datastructures.enums import WorldMode, JointType from ..datastructures.pose import Pose from ..description import Link, Joint @@ -367,7 +367,7 @@ def get_object_contact_points(self, obj: Object) -> List: logging.warning("get_object_contact_points is not implemented in Multiverse") return [] - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List[ContactPoint]: self.check_object_exists_and_issue_warning_if_not(obj1) self.check_object_exists_and_issue_warning_if_not(obj2) logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") From 0809249f59725915eba4882a9fa7b8b6fe60e1a4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 31 May 2024 17:32:12 +0200 Subject: [PATCH 016/189] [Contact] Created a dataclass for ContactPoint and ClosestPoint instead of having them as lists. --- src/pycram/datastructures/dataclasses.py | 6 ++++++ src/pycram/worlds/bullet_world.py | 7 +++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d7f7c7d90..7d9f8ed37 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -316,6 +316,12 @@ class ContactPoint: lateral_friction_1: Optional[LateralFriction] = None lateral_friction_2: Optional[LateralFriction] = None + def __str__(self): + return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}" + + def __repr__(self): + return self.__str__() + @dataclass class ClosestPoint(ContactPoint): diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 25579eab1..38905041b 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -145,14 +145,13 @@ def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, dista points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) return [ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] - @staticmethod - def parse_points_list_to_args(point: List) -> Dict: + def parse_points_list_to_args(self, point: List) -> Dict: """ Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the ContactPoint class. """ - return {"link_a": point[3], - "link_b": point[4], + return {"link_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]), + "link_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]), "position_on_object_a": point[5], "position_on_object_b": point[6], "normal_on_b": point[7], From 21457fb13cc11fe09a7b14f060984fb2faae78f7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 31 May 2024 22:20:57 +0200 Subject: [PATCH 017/189] [TypeHinting] Added type hints for translation and rotation of transform in pose.py. --- src/pycram/datastructures/pose.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 960a4ca19..ee0902633 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -350,7 +350,7 @@ def frame(self, value: str) -> None: self.header.frame_id = value @property - def translation(self) -> None: + def translation(self) -> Vector3: """ Property that points to the translation of this Transform """ @@ -375,7 +375,7 @@ def translation(self, value) -> None: self.transform.translation = value @property - def rotation(self) -> None: + def rotation(self) -> Quaternion: """ Property that points to the rotation of this Transform """ From fc757ead4b667e0133cf53dd1c5d66dba65003d5 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 3 Jun 2024 21:56:03 +0200 Subject: [PATCH 018/189] [Contact] Added ContactPointsList class to the dataclasses. Made it the default type for a list of contact points. Same for ClosestPointsList. --- src/pycram/datastructures/dataclasses.py | 70 ++++++++++++++++++++++- src/pycram/world_concepts/world_object.py | 11 ++-- src/pycram/worlds/bullet_world.py | 17 +++--- src/pycram/worlds/multiverse.py | 8 +-- 4 files changed, 87 insertions(+), 19 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 7d9f8ed37..4f19c358d 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -323,6 +323,70 @@ def __repr__(self): return self.__str__() -@dataclass -class ClosestPoint(ContactPoint): - pass +ClosestPoint = ContactPoint + + +class ContactPointsList(list): + + def get_normals_of_object(self, obj: Object) -> List[List[float]]: + """ + Gets the normals of the object. + :param obj: An instance of the Object class that represents the object. + :return: A list of float vectors that represent the normals of the object. + """ + return self.get_points_of_object(obj).get_normals() + + def get_normals(self) -> List[List[float]]: + """ + Gets the normals of the points. + :return: A list of float vectors that represent the normals of the contact points. + """ + return [point.normal_on_b for point in self] + + def get_points_of_object(self, obj: Object) -> 'ContactPointsList': + """ + Gets the points of the object. + :param obj: + :return: + """ + return ContactPointsList([point for point in self if point.link_b.object == obj]) + + def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: + """ + Returns the object that is not in the current points list but was in the initial points list. + :param previous_points: The initial points list. + :return: A list of Object instances that represent the objects that got removed. + """ + initial_objects_in_contact = previous_points.get_objects_that_have_points() + current_objects_in_contact = self.get_objects_that_have_points() + return [obj for obj in initial_objects_in_contact if obj not in current_objects_in_contact] + + def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: + """ + Returns the object that is not in the initial points list but is in the current points list. + :param previous_points: The initial points list. + :return: A list of Object instances that represent the new objects. + """ + initial_objects_in_contact = previous_points.get_objects_that_have_points() + current_objects_in_contact = self.get_objects_that_have_points() + return [obj for obj in current_objects_in_contact if obj not in initial_objects_in_contact] + + def is_object_in_the_list(self, obj: Object) -> bool: + """ + Checks if the object is one of the objects that have points in the list. + :param obj: An instance of the Object class that represents the object. + :return: True if the object is in the list, False otherwise. + """ + return obj in self.get_objects_that_have_points() + + def get_objects_that_have_points(self) -> List[Object]: + return [point.link_b.object for point in self] + + def __str__(self): + return f"ContactPointsList: {len(self)} contact points" + + def __repr__(self): + return self.__str__() + + +ClosestPointsList = ContactPointsList diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 68cff1c1e..5ae09356c 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -9,7 +9,8 @@ from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape, ClosestPoint) + AxisAlignedBoundingBox, VisualShape, ClosestPoint, ClosestPointsList, + ContactPointsList) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..description import ObjectDescription, LinkDescription @@ -930,7 +931,7 @@ def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> for link in self.links.values(): link.update_transform(transform_time) - def contact_points(self) -> List: + def contact_points(self) -> ContactPointsList: """ Returns a list of contact points of this Object with other Objects. @@ -938,7 +939,7 @@ def contact_points(self) -> List: """ return self.world.get_object_contact_points(self) - def contact_points_simulated(self) -> List: + def contact_points_simulated(self) -> ContactPointsList: """ Returns a list of all contact points between this Object and other Objects after stepping the simulation once. @@ -950,7 +951,7 @@ def contact_points_simulated(self) -> List: self.world.restore_state(state_id) return contact_points - def closest_points(self, max_distance: float) -> List[ClosestPoint]: + def closest_points(self, max_distance: float) -> ClosestPointsList: """ Returns a list of closest points between this Object and other Objects. @@ -959,7 +960,7 @@ def closest_points(self, max_distance: float) -> List[ClosestPoint]: """ return self.world.get_object_closest_points(self, max_distance) - def closest_points_with_obj(self, other_object: Object, max_distance: float) -> List[ClosestPoint]: + def closest_points_with_obj(self, other_object: Object, max_distance: float) -> ClosestPointsList: """ Returns a list of closest points between this Object and another Object. diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 38905041b..42d480fd4 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -12,7 +12,7 @@ from typing_extensions import List, Optional, Dict from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ - ClosestPoint, LateralFriction, ContactPoint + ClosestPoint, LateralFriction, ContactPoint, ContactPointsList, ClosestPointsList from ..datastructures.enums import ObjectType, WorldMode, JointType from ..datastructures.pose import Pose from ..object_descriptors.urdf import ObjectDescription @@ -126,7 +126,7 @@ def get_object_number_of_links(self, obj: Object) -> int: def perform_collision_detection(self) -> None: p.performCollisionDetection(physicsClientId=self.id) - def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: + def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ For a more detailed explanation of the returned list please look at: @@ -134,16 +134,19 @@ def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: """ self.perform_collision_detection() points_list = p.getContactPoints(obj.id, physicsClientId=self.id) - return [ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] + return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) - def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> List[ContactPoint]: + def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> ContactPointsList: self.perform_collision_detection() points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id) - return [ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] + return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) - def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> List[ClosestPoint]: + def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> ClosestPointsList: points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id) - return [ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list if len(point) > 0] + return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list + if len(point) > 0]) def parse_points_list_to_args(self, point: List) -> Dict: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b50356033..b293516d8 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,7 +4,7 @@ from typing_extensions import List, Dict, Optional -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPoint +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPoint, ContactPointsList from ..datastructures.enums import WorldMode, JointType from ..datastructures.pose import Pose from ..description import Link, Joint @@ -362,16 +362,16 @@ def _send_api_request(self): def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") - def get_object_contact_points(self, obj: Object) -> List: + def get_object_contact_points(self, obj: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_object_contact_points is not implemented in Multiverse") return [] - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List[ContactPoint]: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj1) self.check_object_exists_and_issue_warning_if_not(obj2) logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") - return [] + return ContactPointsList([]) def ray_test(self, from_position: List[float], to_position: List[float]) -> int: logging.error("ray_test is not implemented in Multiverse") From a32ebebcc5cfc715a84611c854693f835926d7b5 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 4 Jun 2024 02:18:23 +0200 Subject: [PATCH 019/189] [Contact] Get link of object in contact. --- src/pycram/datastructures/dataclasses.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 4f19c358d..8ca675c57 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -343,6 +343,14 @@ def get_normals(self) -> List[List[float]]: """ return [point.normal_on_b for point in self] + def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: + """ + Gets the links in contact of the object. + :param obj: An instance of the Object class that represents the object. + :return: A list of Link instances that represent the links in contact of the object. + """ + return [point.link_b for point in self if point.link_b.object == obj] + def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ Gets the points of the object. @@ -382,6 +390,9 @@ def is_object_in_the_list(self, obj: Object) -> bool: def get_objects_that_have_points(self) -> List[Object]: return [point.link_b.object for point in self] + def get_names_of_objects_that_have_points(self) -> List[str]: + return [point.link_b.object.name for point in self] + def __str__(self): return f"ContactPointsList: {len(self)} contact points" From 4bb6f7e7ac82bd147aea55bcb31864dd503b8906 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 4 Jun 2024 10:27:51 +0200 Subject: [PATCH 020/189] [AbstractWorld] Added get object names. [Contact] Added ContatPointsList and ClosestPointsList and used them instead of List[ContactPoints], and List[ClosestPoints]. --- src/pycram/world.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/pycram/world.py b/src/pycram/world.py index 18885aac2..ce55f0a50 100644 --- a/src/pycram/world.py +++ b/src/pycram/world.py @@ -18,7 +18,8 @@ from .datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState, ContactPoint, ClosestPoint) + ObjectState, State, WorldState, ContactPoint, ClosestPoint, ClosestPointsList, + ContactPointsList) from .datastructures.enums import JointType, ObjectType, WorldMode from .datastructures.pose import Pose, Transform from .local_transformer import LocalTransformer @@ -264,6 +265,14 @@ def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose """ pass + def get_object_names(self) -> List[str]: + """ + Returns the names of all objects in the World. + + :return: A list of object names. + """ + return [obj.name for obj in self.objects] + def get_object_by_name(self, name: str) -> List[Object]: """ Returns a list of all Objects in this World with the same name as the given one. @@ -452,7 +461,7 @@ def perform_collision_detection(self) -> None: pass @abstractmethod - def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: + def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Returns a list of contact points of this Object with all other Objects. @@ -462,7 +471,7 @@ def get_object_contact_points(self, obj: Object) -> List[ContactPoint]: pass @abstractmethod - def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> List[ContactPoint]: + def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ Returns a list of contact points between obj_a and obj_b. @@ -472,7 +481,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> """ pass - def get_object_closest_points(self, obj: Object, max_distance: float) -> List[ClosestPoint]: + def get_object_closest_points(self, obj: Object, max_distance: float) -> ClosestPointsList: """ Returns the closest points of this object with all other objects in the world. @@ -483,10 +492,10 @@ def get_object_closest_points(self, obj: Object, max_distance: float) -> List[Cl all_obj_closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in self.objects if other_obj != obj] - return [point for closest_points in all_obj_closest_points for point in closest_points] + return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points]) def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \ - -> List[ClosestPoint]: + -> ClosestPointsList: """ Returns the closest points between two objects. From ea8916af0e63bf95993583448e6bbb099da5dab4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 4 Jun 2024 13:13:55 +0200 Subject: [PATCH 021/189] [EventLogger] Implement an Event Text and Color Annotator. --- src/pycram/datastructures/dataclasses.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 8ca675c57..16a21a575 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -401,3 +401,11 @@ def __repr__(self): ClosestPointsList = ContactPointsList + + +@dataclass +class TextAnnotation: + text: str + position: List[float] + color: Color + id: int From afb95ff320f84c73995a6a774143f2f82acb7cd6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 4 Jun 2024 13:15:07 +0200 Subject: [PATCH 022/189] [TextAnnotation] Added a dataclass for text annotations to help replicate or modify them. --- src/pycram/datastructures/dataclasses.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 16a21a575..dbccea7e1 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -402,7 +402,6 @@ def __repr__(self): ClosestPointsList = ContactPointsList - @dataclass class TextAnnotation: text: str From df51f06e0410e9f7297ac4c82b235f2a9ec069a0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 4 Jun 2024 13:15:56 +0200 Subject: [PATCH 023/189] [TextAnnotation] Added a dataclass for text annotations to help replicate or modify them. --- src/pycram/datastructures/dataclasses.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index dbccea7e1..16a21a575 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -402,6 +402,7 @@ def __repr__(self): ClosestPointsList = ContactPointsList + @dataclass class TextAnnotation: text: str From d0864fcf4877f08f278b928ac2766748e6ae21c5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 7 Jun 2024 22:39:38 +0200 Subject: [PATCH 024/189] Publish to pypi --- src/pycram/external_interfaces/ik.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index a6c73f328..830bed498 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -6,10 +6,10 @@ from moveit_msgs.srv import GetPositionIK from sensor_msgs.msg import JointState -from pycram.world_concepts.world_object import Object +from ..world_concepts.world_object import Object from ..helper import calculate_wrist_tool_offset, _apply_ik -from pycram.local_transformer import LocalTransformer -from pycram.datastructures.pose import Pose +from ..local_transformer import LocalTransformer +from ..datastructures.pose import Pose from ..robot_descriptions import robot_description from ..plan_failures import IKError From a188c5b4a56101f7cbd1a8cf512d5e65e14679ed Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 20 Jun 2024 17:35:29 +0200 Subject: [PATCH 025/189] [Multiverse] test attachment, not working. --- test/test_multiverse.py | 43 +++++++++++++++++++++++++++-------------- 1 file changed, 29 insertions(+), 14 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 73ab8dc6c..8d0d26568 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -42,6 +42,7 @@ def setUpClass(cls): is_prospection=True) cls.big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", pose=Pose([2, 2, 1], [0, 0, 0, 1])) + time.sleep(2) @classmethod def tearDownClass(cls): @@ -50,6 +51,7 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.reset_world() # self.multiverse.multiverse_reset_world() + time.sleep(2) pass def test_reset_world(self): @@ -60,25 +62,31 @@ def test_reset_world(self): self.assertAlmostEqual(bowl_position[i], set_position[i]) self.multiverse.reset_world() big_bowl_pose = self.big_bowl.get_pose() - self.assertAlmostEqual(big_bowl_pose, self.big_bowl.original_pose) + for v1, v2 in zip(big_bowl_pose.position_as_list()[:2], self.big_bowl.original_pose.position_as_list()[:2]): + self.assertAlmostEqual(v1, v2, delta=0.001) + for v1, v2 in zip(big_bowl_pose.orientation_as_list(), self.big_bowl.original_pose.orientation_as_list()): + self.assertAlmostEqual(v1, v2, delta=0.001) def test_spawn_object(self): - milk = self.spawn_milk() + milk = self.spawn_milk([1, 0, 0]) self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() - self.assertAlmostEqual(milk_pose, milk.original_pose) + for v1, v2 in zip(milk_pose.position_as_list()[:2], [1, 0]): + self.assertAlmostEqual(v1, v2, delta=0.002) + for v1, v2 in zip(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()): + self.assertAlmostEqual(v1, v2, delta=0.001) def test_remove_object(self): - milk = self.spawn_milk() + milk = self.spawn_milk([0, 0, 2]) milk.remove() def test_check_object_exists(self): - milk = self.spawn_milk() + milk = self.spawn_milk([0, 0, 2]) data = self.multiverse.get_all_objects_data_from_server() self.assertTrue(milk.name in data) def test_set_position(self): - milk = self.spawn_milk() + milk = self.spawn_milk([0, 0, 2]) original_milk_position = milk.get_position_as_list() original_milk_position[0] += 1 milk.set_position(original_milk_position) @@ -86,14 +94,17 @@ def test_set_position(self): self.assertAlmostEqual(milk_position, original_milk_position) def test_update_position(self): - milk = self.spawn_milk() + milk = self.spawn_milk([1, 0, 2]) milk.update_pose() milk_position = milk.get_position_as_list() - for i, v in enumerate([0, 0, 2]): - self.assertAlmostEqual(milk_position[i], v) + for i, v in enumerate([1, 0]): + self.assertAlmostEqual(milk_position[i], v, delta=0.002) def test_set_joint_position(self): - robot = self.spawn_robot() + if self.multiverse.robot is None: + robot = self.spawn_robot() + else: + robot = self.multiverse.robot original_joint_position = robot.get_joint_position("joint1") step = 1.57 i = 0 @@ -108,7 +119,10 @@ def test_set_joint_position(self): self.assertAlmostEqual(joint_position, original_joint_position - 1.57) def test_spawn_robot(self): - robot = self.spawn_robot() + if self.multiverse.robot is not None: + robot = self.multiverse.robot + else: + robot = self.spawn_robot() self.assertIsInstance(robot, Object) self.assertTrue(robot in self.multiverse.objects) self.assertTrue(self.multiverse.robot.name == robot.name) @@ -136,8 +150,9 @@ def test_set_robot_position(self): self.multiverse.robot.set_position(new_position) self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) + @unittest.skip("Not working yet.") def test_attach_object(self): - milk = self.spawn_milk() + milk = self.spawn_milk([1, 0, 1]) milk.attach(self.big_bowl) self.assertTrue(self.big_bowl in milk.attachments) milk_position = milk.get_position_as_list() @@ -167,9 +182,9 @@ def test_attach_with_robot(self): self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) @staticmethod - def spawn_milk() -> Object: + def spawn_milk(position: List) -> Object: milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", - pose=Pose([0, 0, 2], [0, 0, 0, 1])) + pose=Pose(position, [0, 0, 0, 1])) time.sleep(2) return milk From 4423c188b479e9666acd38571500ca812bcb40ca Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 20 Jun 2024 21:39:57 +0200 Subject: [PATCH 026/189] [Multiverse] added lock to viz_marker, added method to remove objects when resetting, synced with current pycram dev. --- src/pycram/datastructures/world.py | 22 +++++++++++++++------- src/pycram/ros/viz_marker_publisher.py | 4 +++- src/pycram/world_concepts/world_object.py | 7 +++---- src/pycram/world_reasoning.py | 11 ++++++----- src/pycram/worlds/multiverse.py | 2 +- 5 files changed, 28 insertions(+), 18 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 886b4be3e..376b558c1 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -15,17 +15,17 @@ from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING from typing_extensions import Union -from .cache_manager import CacheManager -from .datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, +from ..cache_manager import CacheManager +from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, State, WorldState, ContactPoint, ClosestPoint, ClosestPointsList, ContactPointsList) -from .datastructures.enums import JointType, ObjectType, WorldMode -from .datastructures.pose import Pose, Transform -from .local_transformer import LocalTransformer -from .world_concepts.constraints import Constraint -from .world_concepts.event import Event +from ..datastructures.enums import JointType, ObjectType, WorldMode +from ..datastructures.pose import Pose, Transform +from ..local_transformer import LocalTransformer +from ..world_concepts.constraints import Constraint +from ..world_concepts.event import Event if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -843,6 +843,14 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec except ValueError: raise ValueError("The given object is not in the prospection world.") + def reset_world_and_remove_objects(self) -> None: + """ + Resets the World to the state it was first spawned in and removes all objects from the World. + """ + self.reset_world() + for obj in copy(self.objects): + self.remove_object(obj) + def reset_world(self, remove_saved_states=True) -> None: """ Resets the World to the state it was first spawned in. diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 0aa149e9b..ee87827ea 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -34,6 +34,7 @@ def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): self.thread = threading.Thread(target=self._publish) self.kill_event = threading.Event() + self.lock = threading.Lock() self.main_world = World.current_world if not World.current_world.is_prospection_world else World.current_world.world_sync.world self.thread.start() @@ -44,8 +45,9 @@ def _publish(self) -> None: Constantly publishes the Marker Array. To the given topic name at a fixed rate. """ while not self.kill_event.is_set(): + self.lock.acquire() marker_array = self._make_marker_array() - + self.lock.release() self.pub.publish(marker_array) time.sleep(self.interval) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 434071e35..79ac300e8 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,13 +8,12 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, - AxisAlignedBoundingBox, VisualShape, ClosestPoint, ClosestPointsList, + AxisAlignedBoundingBox, VisualShape, ClosestPointsList, ContactPointsList) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform -from ..description import ObjectDescription, LinkDescription +from ..description import ObjectDescription, LinkDescription, Joint from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_descriptions import robot_description @@ -62,7 +61,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if pose is None: pose = Pose() - if name in [obj.name for obj in self.world.objects]: + if name in self.world.get_object_names(): rospy.logerr(f"An object with the name {name} already exists in the world.") return None self.name: str = name diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 30976dbee..35a2b0527 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -5,9 +5,10 @@ from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .datastructures.pose import Pose, Transform +from .datastructures.dataclasses import ContactPointsList from .robot_descriptions import robot_description -from .world_concepts.world_object import Object, Link -from .world import World, UseProspectionWorld +from .world_concepts.world_object import Object +from .datastructures.world import World, UseProspectionWorld def stable(obj: Object) -> bool: @@ -52,13 +53,13 @@ def contact( prospection_obj2 = World.current_world.get_prospection_object_for_object(object2) World.current_world.perform_collision_detection() - con_points = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) + con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1, + prospection_obj2) if return_links: contact_links = [] for point in con_points: - contact_links.append((prospection_obj1.get_link_by_id(point[3]), - prospection_obj2.get_link_by_id(point[4]))) + contact_links.append((point.link_a, point.link_b)) return con_points != (), contact_links else: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b293516d8..43e141bdb 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -8,7 +8,7 @@ from ..datastructures.enums import WorldMode, JointType from ..datastructures.pose import Pose from ..description import Link, Joint -from ..world import World +from ..datastructures.world import World from ..world_concepts.constraints import Constraint from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress from ..world_concepts.world_object import Object From cfe524e5cbe4236e36b328ed2e6d786b971c4165 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 20 Jun 2024 21:54:16 +0200 Subject: [PATCH 027/189] [Multiverse] fixed import multiverse test. --- test/test_multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 8d0d26568..b3810db14 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -7,7 +7,7 @@ from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose -from pycram.world import UseProspectionWorld +from pycram.datastructures.world import UseProspectionWorld from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse From faca2300478ffee2591445d682d9e4495d7cfe0b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 16:39:47 +0200 Subject: [PATCH 028/189] [Multiverse] fixed attach issue. --- src/pycram/description.py | 9 +++ src/pycram/world_concepts/world_object.py | 7 ++ src/pycram/worlds/multiverse.py | 83 +++++++++++++++-------- test/test_location_designator.py | 3 +- test/test_multiverse.py | 69 ++++++++++--------- 5 files changed, 110 insertions(+), 61 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index ccd1804ec..fc1b19c0e 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -239,6 +239,15 @@ def remove_constraint_with_link(self, child_link: 'Link') -> None: if self in child_link.constraint_ids.keys(): del child_link.constraint_ids[self] + @property + def is_only_link(self) -> bool: + """ + Returns whether this link is the only link of the object. + + :return: True if this link is the only link, False otherwise. + """ + return self.object.has_one_link + @property def is_root(self) -> bool: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 79ac300e8..3a8dcf92d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -189,6 +189,13 @@ def _add_to_world_sync_obj_queue(self) -> None: """ self.world.world_sync.add_obj_queue.put(self) + @property + def has_one_link(self) -> bool: + """ + Returns True if the object has only one link, otherwise False. + """ + return len(self.links) == 1 + @property def link_names(self) -> List[str]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 43e141bdb..a819f01bc 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -2,13 +2,13 @@ import os from time import time -from typing_extensions import List, Dict, Optional +from typing_extensions import List, Dict, Optional, Tuple -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPoint, ContactPointsList +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList from ..datastructures.enums import WorldMode, JointType from ..datastructures.pose import Pose -from ..description import Link, Joint from ..datastructures.world import World +from ..description import Link, Joint from ..world_concepts.constraints import Constraint from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress from ..world_concepts.world_object import Object @@ -22,7 +22,7 @@ def get_resource_paths(dirname: str) -> List[str]: for resources_path in resources_paths ] - def add_directories(path: str) -> List[str]: + def add_directories(path: str) -> None: with os.scandir(path) as entries: for entry in entries: if entry.is_dir(): @@ -226,7 +226,9 @@ def _get_body_pose(self, body_name: str) -> Pose: if len(self.receive_data) != 8: logging.error(f"Invalid body pose data: {self.receive_data}") raise ValueError - return Pose(self.receive_data[1:4], self.receive_data[4:]) + wxyz = self.receive_data[4:] + xyzw = [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] + return Pose(self.receive_data[1:4], xyzw) def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) @@ -247,7 +249,9 @@ def _reset_body_pose(self, body_name: str, pose: Pose) -> None: self._init_setter() self.request_meta_data["send"][body_name] = ["position", "quaternion"] self.send_and_receive_meta_data() - self.send_data = [time() - self.time_start, *pose.position_as_list(), *pose.orientation_as_list()] + xyzw = pose.orientation_as_list() + wxyz = [xyzw[3], *xyzw[:3]] + self.send_data = [time() - self.time_start, *pose.position_as_list(), *wxyz] self.send_and_receive_data() def get_all_objects_data_from_server(self) -> Dict[str, Dict]: @@ -277,37 +281,60 @@ def add_constraint(self, constraint: Constraint) -> int: if constraint.type != JointType.FIXED: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError + self._request_attach(constraint) + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) constraint_id = self.last_constraint_id + 1 - parent_link_name = self.get_link_name_from_constraint_link(constraint.parent_link) - child_link_name = self.get_link_name_from_constraint_link(constraint.child_link) - self._add_api_request("attach", parent_link_name, - child_link_name, self._get_attachment_pose_as_string(constraint)) - self._send_api_request() self.constraints[constraint_id] = {'parent_link': parent_link_name, 'child_link': child_link_name} return constraint_id + def _request_attach(self, constraint: Constraint) -> None: + """ + Request to attach the child link to the parent link. + param constraint: The constraint. + """ + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + self._add_api_request("attach", child_link_name, + parent_link_name, self._get_attachment_pose_as_string(constraint)) + self._send_api_request() + + def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: + """ + Get the link names of the constraint. + param constraint: The constraint. + return: The link names of the constraint. + """ + return self.get_parent_link_name(constraint), self.get_constraint_child_link_name(constraint) + + def get_parent_link_name(self, constraint: Constraint) -> str: + """ + Get the parent link name of the constraint. + param constraint: The constraint. + return: The parent link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.parent_link) + + def get_constraint_child_link_name(self, constraint: Constraint) -> str: + """ + Get the child link name of the constraint. + param constraint: The constraint. + return: The child link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.child_link) + def remove_constraint(self, constraint_id) -> None: constraint = self.constraints.pop(constraint_id) self._add_api_request("detach", constraint['parent_link'], constraint['child_link']) self._send_api_request() - def get_link_name_from_constraint_link(self, link: Link) -> str: + @staticmethod + def get_link_name_for_constraint(link: Link) -> str: """ - Get the link name from the constraint link, if the link belongs to a one link object, return the object name. + Get the link name from link object, if the link belongs to a one link object, return the object name. param link: The link. return: The link name. """ - return link.name if not self.is_one_link_object(link.object) else link.object.name - - @staticmethod - def is_one_link_object(obj: Object) -> bool: - """ - Check if the object has only one link. - param obj: The object. - return: True if the object has only one link, False otherwise. - """ - return len(obj.links) == 1 + return link.name if not link.is_only_link else link.object.name def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: """ @@ -328,7 +355,7 @@ def _pose_to_string(pose: Pose) -> str: return: The pose as a string. """ return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ - f"{pose.orientation.y} {pose.orientation.z}" + f"{pose.orientation.y} {pose.orientation.z}" def _init_api_callback(self): """ @@ -365,7 +392,7 @@ def perform_collision_detection(self) -> None: def get_object_contact_points(self, obj: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_object_contact_points is not implemented in Multiverse") - return [] + return ContactPointsList([]) def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj1) @@ -426,6 +453,6 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") - def check_object_exists_and_issue_warning_if_not(self, object): - if object not in self.objects: - logging.warning(f"Object {object.name} does not exist in the simulator") + def check_object_exists_and_issue_warning_if_not(self, obj: Object): + if obj not in self.objects: + logging.warning(f"Object {obj.name} does not exist in the simulator") diff --git a/test/test_location_designator.py b/test/test_location_designator.py index 714f8ad54..67f3ef195 100644 --- a/test/test_location_designator.py +++ b/test/test_location_designator.py @@ -18,7 +18,8 @@ def test_reachability(self): def test_reachability_pose(self): robot_desig = ObjectDesignatorDescription(names=[robot_description.name]) - location_desig = CostmapLocation(Pose([0.4, 0.6, 0.9], [0, 0, 0, 1]), reachable_for=robot_desig.resolve()) + location_desig = CostmapLocation(Pose([0.4, 0.6, 0.9], [0, 0, 0, 1]), + reachable_for=robot_desig.resolve()) location = location_desig.resolve() self.assertTrue(len(location.pose.position_as_list()) == 3) self.assertTrue(len(location.pose.orientation_as_list()) == 4) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b3810db14..77e101818 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -11,12 +11,6 @@ from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse import Multiverse -multiverse_installed = True -try: - from pycram.world_concepts.multiverse_socket import MultiverseMetaData, SocketAddress -except ImportError: - multiverse_installed = False - processes = psutil.process_iter() process_names = [p.name() for p in processes] multiverse_running = True @@ -27,7 +21,6 @@ mujoco_running = False -@unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") @unittest.skipIf(not multiverse_running, "Multiverse server is not running.") @unittest.skipIf(not mujoco_running, "Mujoco is not running.") # @unittest.skip("Needs Multiverse server and simulation to be running") @@ -37,25 +30,28 @@ class MultiversePyCRAMTestCase(unittest.TestCase): @classmethod def setUpClass(cls): - cls.multiverse = Multiverse(simulation="pycram_test", - client_addr=SocketAddress(port="5481"), - is_prospection=True) - cls.big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", - pose=Pose([2, 2, 1], [0, 0, 0, 1])) - time.sleep(2) + try: + from pycram.world_concepts.multiverse_socket import SocketAddress + cls.multiverse = Multiverse(simulation="pycram_test", + client_addr=SocketAddress(port="5481"), + is_prospection=True) + except ImportError: + return + cls.big_bowl = cls.spawn_big_bowl() @classmethod def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() def tearDown(self): - self.multiverse.reset_world() + self.multiverse.reset_world_and_remove_objects() # self.multiverse.multiverse_reset_world() - time.sleep(2) + MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() + time.sleep(0.5) pass def test_reset_world(self): - set_position = [1, 1, 0] + set_position = [1, 1, 0.1] self.big_bowl.set_position(set_position) bowl_position = self.big_bowl.get_position_as_list() for i in range(3): @@ -68,33 +64,34 @@ def test_reset_world(self): self.assertAlmostEqual(v1, v2, delta=0.001) def test_spawn_object(self): - milk = self.spawn_milk([1, 0, 0]) + milk = self.spawn_milk([1, 0, 0.1]) self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() for v1, v2 in zip(milk_pose.position_as_list()[:2], [1, 0]): self.assertAlmostEqual(v1, v2, delta=0.002) for v1, v2 in zip(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()): - self.assertAlmostEqual(v1, v2, delta=0.001) + self.assertAlmostEqual(v1, v2, delta=0.002) def test_remove_object(self): - milk = self.spawn_milk([0, 0, 2]) + milk = self.spawn_milk([0, 0, 0.1]) milk.remove() def test_check_object_exists(self): - milk = self.spawn_milk([0, 0, 2]) + milk = self.spawn_milk([0, 0, 0.1]) data = self.multiverse.get_all_objects_data_from_server() self.assertTrue(milk.name in data) def test_set_position(self): - milk = self.spawn_milk([0, 0, 2]) + milk = self.spawn_milk([0, 0, 0.1]) original_milk_position = milk.get_position_as_list() original_milk_position[0] += 1 milk.set_position(original_milk_position) milk_position = milk.get_position_as_list() - self.assertAlmostEqual(milk_position, original_milk_position) + for v1, v2 in zip(milk_position[:2], original_milk_position[:2]): + self.assertAlmostEqual(v1, v2, delta=0.002) def test_update_position(self): - milk = self.spawn_milk([1, 0, 2]) + milk = self.spawn_milk([1, 0, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() for i, v in enumerate([1, 0]): @@ -135,24 +132,24 @@ def test_destroy_robot(self): self.assertTrue(self.multiverse.robot not in self.multiverse.objects) @unittest.skip("Not implemented feature yet.") - def test_respawmn_robot(self): + def test_respawn_robot(self): self.spawn_robot() self.assertTrue(self.multiverse.robot in self.multiverse.objects) self.multiverse.robot.remove() self.assertTrue(self.multiverse.robot not in self.multiverse.objects) - self.spawn_robot(position=[0, 0, 1]) + self.spawn_robot(position=[0, 0, 0.1]) self.assertTrue(self.multiverse.robot in self.multiverse.objects) def test_set_robot_position(self): if self.multiverse.robot is None: self.spawn_robot() - new_position = [1, 1, 0] + new_position = [1, 1, 0.1] self.multiverse.robot.set_position(new_position) self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) - @unittest.skip("Not working yet.") + # @unittest.skip("Not working yet.") def test_attach_object(self): - milk = self.spawn_milk([1, 0, 1]) + milk = self.spawn_milk([1, 0, 0.1]) milk.attach(self.big_bowl) self.assertTrue(self.big_bowl in milk.attachments) milk_position = milk.get_position_as_list() @@ -161,8 +158,9 @@ def test_attach_object(self): estimated_bowl_position = big_bowl_position.copy() estimated_bowl_position[0] += 1 milk.set_position(milk_position) + time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. new_bowl_position = self.big_bowl.get_position_as_list() - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) @unittest.skip("Needs mobile base robot") def test_attach_with_robot(self): @@ -181,18 +179,25 @@ def test_attach_with_robot(self): estimated_bowl_position[2] += 3 self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + @staticmethod + def spawn_big_bowl() -> Object: + big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", + pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) + time.sleep(0.5) + return big_bowl + @staticmethod def spawn_milk(position: List) -> Object: milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(2) + time.sleep(0.5) return milk @staticmethod def spawn_robot(position: Optional[List[float]] = None) -> Object: if position is None: - position = [0, 0, 0] + position = [0, 0, 0.1] robot = Object("panda", ObjectType.ROBOT, "panda.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(2) + time.sleep(0.5) return robot From 1b1c0aef3664654504ec87c92a8dbd2159d04e1c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 16:47:23 +0200 Subject: [PATCH 029/189] [Multiverse] fixed import issue when multiverse is not installed. --- test/test_multiverse.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 77e101818..ba5393d66 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -9,7 +9,13 @@ from pycram.datastructures.pose import Pose from pycram.datastructures.world import UseProspectionWorld from pycram.world_concepts.world_object import Object -from pycram.worlds.multiverse import Multiverse + +multiverse_installed = True +try: + from pycram.worlds.multiverse import Multiverse + from pycram.world_concepts.multiverse_socket import SocketAddress +except ImportError: + multiverse_installed = False processes = psutil.process_iter() process_names = [p.name() for p in processes] @@ -21,6 +27,7 @@ mujoco_running = False +@unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") @unittest.skipIf(not multiverse_running, "Multiverse server is not running.") @unittest.skipIf(not mujoco_running, "Mujoco is not running.") # @unittest.skip("Needs Multiverse server and simulation to be running") @@ -30,13 +37,9 @@ class MultiversePyCRAMTestCase(unittest.TestCase): @classmethod def setUpClass(cls): - try: - from pycram.world_concepts.multiverse_socket import SocketAddress - cls.multiverse = Multiverse(simulation="pycram_test", - client_addr=SocketAddress(port="5481"), - is_prospection=True) - except ImportError: - return + cls.multiverse = Multiverse(simulation="pycram_test", + client_addr=SocketAddress(port="5481"), + is_prospection=True) cls.big_bowl = cls.spawn_big_bowl() @classmethod From 54c2c905a33b3cbfb6bd1357f5b4180c4e7d27dc Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 17:02:15 +0200 Subject: [PATCH 030/189] [Multiverse] fixed import issue when multiverse is not installed. --- test/test_multiverse.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ba5393d66..aa32f23aa 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -50,7 +50,7 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() # self.multiverse.multiverse_reset_world() MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() - time.sleep(0.5) + time.sleep(1) pass def test_reset_world(self): @@ -202,5 +202,5 @@ def spawn_robot(position: Optional[List[float]] = None) -> Object: position = [0, 0, 0.1] robot = Object("panda", ObjectType.ROBOT, "panda.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(0.5) + time.sleep(1) return robot From aca38a2e9d689ae321d2cb1e4291341d10e258a3 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 17:10:59 +0200 Subject: [PATCH 031/189] [Multiverse] fixed import issue when multiverse is not installed. --- test/test_multiverse.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index aa32f23aa..35f3687d7 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -30,13 +30,15 @@ @unittest.skipIf(not multiverse_installed, "Multiverse is not installed.") @unittest.skipIf(not multiverse_running, "Multiverse server is not running.") @unittest.skipIf(not mujoco_running, "Mujoco is not running.") -# @unittest.skip("Needs Multiverse server and simulation to be running") class MultiversePyCRAMTestCase(unittest.TestCase): - multiverse: Multiverse + if multiverse_installed: + multiverse: Multiverse big_bowl: Optional[Object] = None @classmethod def setUpClass(cls): + if not multiverse_installed: + return cls.multiverse = Multiverse(simulation="pycram_test", client_addr=SocketAddress(port="5481"), is_prospection=True) From 00190310e8b73dbef4f6a91ca6c63bda057864d2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 17:48:56 +0200 Subject: [PATCH 032/189] [Multiverse] Updated object lock to be defined locally in the world class instead of vizmarker. Acquire and release of lock when removing any object. --- src/pycram/datastructures/world.py | 7 ++++++- src/pycram/ros/viz_marker_publisher.py | 5 +++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 376b558c1..fb31141d3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -135,7 +135,7 @@ class World(StateEntity, ABC): and the objects. """ - cache_dir = data_directory[0] + '/cached/' + cache_dir: str = data_directory[0] + '/cached/' """ Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ @@ -157,6 +157,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ World.simulation_frequency = simulation_frequency self.cache_manager = CacheManager(self.cache_dir, self.data_directory) + self.object_lock: threading.Lock = threading.Lock() self.id: Optional[int] = -1 # This is used to connect to the physics server (allows multiple clients) @@ -319,6 +320,9 @@ def remove_object(self, obj: Object) -> None: :param obj: The object to be removed. """ + while self.object_lock.locked(): + time.sleep(0.1) + self.object_lock.acquire() obj.detach_all() self.objects.remove(obj) @@ -333,6 +337,7 @@ def remove_object(self, obj: Object) -> None: if World.robot == obj: World.robot = None + self.object_lock.release() def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index ee87827ea..1aa2b0e09 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -34,9 +34,8 @@ def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): self.thread = threading.Thread(target=self._publish) self.kill_event = threading.Event() - self.lock = threading.Lock() self.main_world = World.current_world if not World.current_world.is_prospection_world else World.current_world.world_sync.world - + self.lock = self.main_world.object_lock self.thread.start() atexit.register(self._stop_publishing) @@ -45,6 +44,8 @@ def _publish(self) -> None: Constantly publishes the Marker Array. To the given topic name at a fixed rate. """ while not self.kill_event.is_set(): + while self.lock.locked(): + time.sleep(0.1) self.lock.acquire() marker_array = self._make_marker_array() self.lock.release() From fbfb92ba565e2aae1046abadb6d382179f275a9d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 21 Jun 2024 20:50:52 +0200 Subject: [PATCH 033/189] [Multiverse] Added and tested get contact points feature, Currently it gets only the contact bodies not the points. --- src/pycram/datastructures/dataclasses.py | 2 +- src/pycram/datastructures/world.py | 14 ++- src/pycram/worlds/multiverse.py | 153 ++++++++++++++++------- test/test_multiverse.py | 32 ++++- 4 files changed, 152 insertions(+), 49 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 16a21a575..701941e86 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -394,7 +394,7 @@ def get_names_of_objects_that_have_points(self) -> List[str]: return [point.link_b.object.name for point in self] def __str__(self): - return f"ContactPointsList: {len(self)} contact points" + return f"ContactPointsList: {', '.join(self.get_names_of_objects_that_have_points())}" def __repr__(self): return self.__str__() diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index fb31141d3..8108abab3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -275,14 +275,18 @@ def get_object_names(self) -> List[str]: """ return [obj.name for obj in self.objects] - def get_object_by_name(self, name: str) -> List[Object]: + def get_object_by_name(self, name: str) -> Optional[Object]: """ - Returns a list of all Objects in this World with the same name as the given one. + Returns the object in this World with the same name as the given one. - :param name: The name of the returned Objects. - :return: A list of all Objects with the name 'name'. + :param name: The name of the returned object. + :return: the object with the name 'name'. """ - return list(filter(lambda obj: obj.name == name, self.objects))[0] + obj = [obj for obj in self.objects if obj.name == name] + if len(obj) == 0: + return None + else: + return obj[0] def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a819f01bc..2aa2af3f2 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,14 +1,14 @@ import logging import os -from time import time +from time import time, sleep from typing_extensions import List, Dict, Optional, Tuple -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList -from ..datastructures.enums import WorldMode, JointType +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint +from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose from ..datastructures.world import World -from ..description import Link, Joint +from ..description import Link, Joint, LinkDescription from ..world_concepts.constraints import Constraint from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress from ..world_concepts.world_object import Object @@ -114,11 +114,12 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT self.update_poses_on_get = True self.last_object_id: int = -1 self.last_constraint_id: int = -1 - self.constraints: Dict[int, Dict[str, str]] = {} + self.constraints: Dict[int, Constraint] = {} self.object_name_to_id: Dict[str, int] = {} self.object_id_to_name: Dict[int, str] = {} self.time_start = time() self.run() + self.floor = self._spawn_floor() def _init_world(self, mode: WorldMode): pass @@ -134,6 +135,15 @@ def _make_sure_multiverse_resources_are_added(self): self.add_resource_path(resource_path) self.added_multiverse_resources = True + def _spawn_floor(self): + """ + Spawn the plane in the simulator. + """ + floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", + world=self) + sleep(0.5) + return floor + def get_joint_position_name(self, joint: Joint) -> str: if joint.type not in self._joint_type_to_position_name: logging.warning(f"Invalid joint type: {joint.type}") @@ -278,15 +288,18 @@ def remove_object_from_simulator(self, obj: Object) -> None: self.send_and_receive_data() def add_constraint(self, constraint: Constraint) -> int: + if constraint.type != JointType.FIXED: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError + self._request_attach(constraint) - parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - constraint_id = self.last_constraint_id + 1 - self.constraints[constraint_id] = {'parent_link': parent_link_name, - 'child_link': child_link_name} - return constraint_id + + self.last_constraint_id += 1 + + self.constraints[self.last_constraint_id] = constraint + + return self.last_constraint_id def _request_attach(self, constraint: Constraint) -> None: """ @@ -324,7 +337,15 @@ def get_constraint_child_link_name(self, constraint: Constraint) -> str: def remove_constraint(self, constraint_id) -> None: constraint = self.constraints.pop(constraint_id) - self._add_api_request("detach", constraint['parent_link'], constraint['child_link']) + self._request_detach(constraint) + + def _request_detach(self, constraint: Constraint) -> None: + """ + Request to detach the child link from the parent link. + param constraint: The constraint. + """ + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + self._add_api_request("detach", child_link_name, parent_link_name) self._send_api_request() @staticmethod @@ -357,42 +378,43 @@ def _pose_to_string(pose: Pose) -> str: return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ f"{pose.orientation.y} {pose.orientation.z}" - def _init_api_callback(self): - """ - Initialize the API callback in the request metadata. - """ - self.request_meta_data["send"] = {} - self.request_meta_data["receive"] = {} - self.request_meta_data["api_callbacks"] = {self.simulation: []} - self.set_simulation_in_request_meta_data() - - def _add_api_request(self, api_name: str, *params): - """ - Add an API request to the request metadata. - param api_name: The name of the API. - param params: The parameters of the API. - """ - if "api_callbacks" not in self.request_meta_data: - self._init_api_callback() - self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) - - def _send_api_request(self): - """ - Send the API request to the server. - """ - if "api_callbacks" not in self.request_meta_data: - logging.error("No API request to send") - raise ValueError - self.send_and_receive_meta_data() - self.request_meta_data.pop("api_callbacks") - def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") def get_object_contact_points(self, obj: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj) - logging.warning("get_object_contact_points is not implemented in Multiverse") - return ContactPointsList([]) + contact_bodies = self._request_objects_in_contact(obj) + contact_points = ContactPointsList([]) + body_link = None + for body in contact_bodies: + if body == "world": + continue + body_object = self.get_object_by_name(body) + if body_object is None: + for obj in self.objects: + for link in obj.links.values(): + if link.name == body: + body_link = link + break + else: + body_link = body_object.root_link + if body_link is None: + logging.error(f"Body link not found: {body}") + raise ValueError + contact_points.append(ContactPoint(obj.root_link, body_link)) + return contact_points + + def _request_objects_in_contact(self, obj: Object) -> List[str]: + self._init_api_callback() + self._add_api_request("get_contact_bodies", obj.name) + self._send_api_request() + return self._get_contact_bodies_from_response() + + def _get_contact_bodies_from_response(self) -> List[str]: + return self._get_api_response("get_contact_bodies") + + def _get_api_response(self, api_name: str) -> List[str]: + return self.response_meta_data["api_callbacks_response"][self.simulation][0][api_name] def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj1) @@ -456,3 +478,50 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: logging.warning(f"Object {obj.name} does not exist in the simulator") + + def _init_api_callback(self): + """ + Initialize the API callback in the request metadata. + """ + self._reset_send_data() + self._reset_receive_data() + self._reset_api_callback() + self.set_simulation_in_request_meta_data() + + def _reset_send_data(self): + """ + Reset the send data in the request metadata. + """ + self.request_meta_data["send"] = {} + + def _reset_receive_data(self): + """ + Reset the receive data in the request metadata. + """ + self.request_meta_data["receive"] = {} + + def _reset_api_callback(self): + """ + Reset the API callback in the request metadata. + """ + self.request_meta_data["api_callbacks"] = {self.simulation: []} + + def _add_api_request(self, api_name: str, *params): + """ + Add an API request to the request metadata. + param api_name: The name of the API. + param params: The parameters of the API. + """ + if "api_callbacks" not in self.request_meta_data: + self._init_api_callback() + self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) + + def _send_api_request(self): + """ + Send the API request to the server. + """ + if "api_callbacks" not in self.request_meta_data: + logging.error("No API request to send") + raise ValueError + self.send_and_receive_meta_data() + self.request_meta_data.pop("api_callbacks") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 35f3687d7..cd07272a8 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -152,7 +152,6 @@ def test_set_robot_position(self): self.multiverse.robot.set_position(new_position) self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) - # @unittest.skip("Not working yet.") def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) milk.attach(self.big_bowl) @@ -167,6 +166,23 @@ def test_attach_object(self): new_bowl_position = self.big_bowl.get_position_as_list() self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + def test_detach_object(self): + milk = self.spawn_milk([1, 0, 0.1]) + milk.attach(self.big_bowl) + self.assertTrue(self.big_bowl in milk.attachments) + milk.detach(self.big_bowl) + self.assertTrue(self.big_bowl not in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 1 + big_bowl_position = self.big_bowl.get_position_as_list() + estimated_bowl_position = big_bowl_position.copy() + milk.set_position(milk_position) + time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. + new_milk_position = milk.get_position_as_list() + new_bowl_position = self.big_bowl.get_position_as_list() + self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + @unittest.skip("Needs mobile base robot") def test_attach_with_robot(self): with UseProspectionWorld(): @@ -184,6 +200,20 @@ def test_attach_with_robot(self): estimated_bowl_position[2] += 3 self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + def test_get_object_contact_points(self): + milk = self.spawn_milk([1, 0, 0.1]) + contact_points = self.multiverse.get_object_contact_points(milk) + self.assertIsInstance(contact_points, list) + self.assertEqual(len(contact_points), 1) + self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) + big_bowl_position = self.big_bowl.get_position_as_list() + milk.set_position([big_bowl_position[0], big_bowl_position[1], big_bowl_position[2] + 0.5]) + time.sleep(0.5) + contact_points = self.multiverse.get_object_contact_points(milk) + self.assertIsInstance(contact_points, list) + self.assertEqual(len(contact_points), 1) + self.assertTrue(contact_points[0].link_b.object, self.big_bowl) + @staticmethod def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", From 2f69400ec31a4ca5397c1ddd3827709bdeabe309 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 25 Jun 2024 15:58:28 +0200 Subject: [PATCH 034/189] [Multiverse] Fixed contact issue in pose_generator_and_validator.py. changed the contact check to be on the length of the list. --- src/pycram/pose_generator_and_validator.py | 5 +++-- src/pycram/world_reasoning.py | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 12115f8fe..93160b948 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -245,8 +245,9 @@ def collision_check(robot: Object, allowed_collision: list): for obj in World.current_world.objects: if obj.name == "floor": continue - in_contact= _in_contact(robot, obj, allowed_collision, allowed_robot_links) - + in_contact = _in_contact(robot, obj, allowed_collision, allowed_robot_links) + if in_contact: + break return in_contact diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 35a2b0527..710e31e26 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -55,15 +55,15 @@ def contact( World.current_world.perform_collision_detection() con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) - + objects_are_in_contact = len(con_points) > 0 if return_links: contact_links = [] for point in con_points: contact_links.append((point.link_a, point.link_b)) - return con_points != (), contact_links + return objects_are_in_contact, contact_links else: - return con_points != () + return objects_are_in_contact def get_visible_objects( From 2c44158d80deb6f5882c33e10e129ef6670de5bd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 25 Jun 2024 19:49:49 +0200 Subject: [PATCH 035/189] [Multiverse] Added contact forces from multiverse. Added a transform property to world object. --- src/pycram/world_concepts/world_object.py | 4 + src/pycram/worlds/multiverse.py | 216 ++++++++++++++++++---- 2 files changed, 181 insertions(+), 39 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 3a8dcf92d..b704dc9a6 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -108,6 +108,10 @@ def pose(self): def pose(self, pose: Pose): self.set_pose(pose) + @property + def transform(self): + return self.get_pose().to_transform(self.tf_frame) + def _load_object_and_get_id(self, path: str, ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 2aa2af3f2..3e0a9eeb6 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,14 +1,17 @@ import logging import os +from dataclasses import dataclass from time import time, sleep from typing_extensions import List, Dict, Optional, Tuple +from tf.transformations import quaternion_matrix +import numpy as np from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose from ..datastructures.world import World -from ..description import Link, Joint, LinkDescription +from ..description import Link, Joint from ..world_concepts.constraints import Constraint from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress from ..world_concepts.world_object import Object @@ -76,6 +79,39 @@ def find_multiverse_path() -> Optional[str]: return None +@dataclass +class APIData: + """ + A dataclass to store the API data. + """ + api_name: str + params: List[str] + + @property + def as_dict(self) -> Dict: + return {self.api_name: self.params} + + +class APIDataDict(dict): + """ + A dictionary to store the API data, where the key is the API name and the value is the parameters. + """ + @classmethod + def from_list(cls, api_data_list: List[APIData]) -> "APIDataDict": + data = {api_data.api_name: api_data.params for api_data in api_data_list} + return cls(data) + + +@dataclass +class MultiverseContactPoint: + """ + A dataclass to store the contact point returned from Multiverse. + """ + body_name: str + contact_force: List[float] + contact_torque: List[float] + + class Multiverse(MultiverseSocket, World): """ This class implements an interface between Multiverse and PyCRAM. @@ -94,6 +130,15 @@ class Multiverse(MultiverseSocket, World): A flag to check if the multiverse resources have been added. """ + GET_CONTACT_BODIES_API_NAME = "get_contact_bodies" + GET_CONSTRAINT_EFFORT_API_NAME = "get_constraint_effort" + ATTACH_API_NAME = "attach" + DETACH_API_NAME = "detach" + GET_RAYS_API_NAME = "get_rays" + """ + The API names for the API callbacks to the Multiverse server. + """ + def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, @@ -140,7 +185,7 @@ def _spawn_floor(self): Spawn the plane in the simulator. """ floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", - world=self) + world=self) sleep(0.5) return floor @@ -208,9 +253,6 @@ def _init_setter(self): self.request_meta_data["receive"] = {} self.set_simulation_in_request_meta_data() - def set_simulation_in_request_meta_data(self): - self.request_meta_data["meta_data"]["simulation_name"] = self.simulation - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: self._init_setter() attribute = self.get_joint_position_name(joint) @@ -383,38 +425,40 @@ def perform_collision_detection(self) -> None: def get_object_contact_points(self, obj: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj) - contact_bodies = self._request_objects_in_contact(obj) + multiverse_contact_points = self._request_contact_points(obj) contact_points = ContactPointsList([]) body_link = None - for body in contact_bodies: - if body == "world": + for point in multiverse_contact_points: + if point.body_name == "world": continue - body_object = self.get_object_by_name(body) + body_object = self.get_object_by_name(point.body_name) if body_object is None: for obj in self.objects: for link in obj.links.values(): - if link.name == body: + if link.name == point.body_name: body_link = link break else: body_link = body_object.root_link if body_link is None: - logging.error(f"Body link not found: {body}") + logging.error(f"Body link not found: {point.body_name}") raise ValueError contact_points.append(ContactPoint(obj.root_link, body_link)) + normal_force = self._get_normal_force_on_object_from_contact_force(obj, point.contact_force) + contact_points[-1].normal_on_b = normal_force + contact_points[-1].normal_force = normal_force return contact_points - def _request_objects_in_contact(self, obj: Object) -> List[str]: - self._init_api_callback() - self._add_api_request("get_contact_bodies", obj.name) - self._send_api_request() - return self._get_contact_bodies_from_response() - - def _get_contact_bodies_from_response(self) -> List[str]: - return self._get_api_response("get_contact_bodies") - - def _get_api_response(self, api_name: str) -> List[str]: - return self.response_meta_data["api_callbacks_response"][self.simulation][0][api_name] + @staticmethod + def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: List[float]) -> float: + """ + Get the normal force on an object from the contact force exerted by another object that is expressed in the + world frame. Thus transforming the contact force to the object frame is necessary. + """ + obj_quat = obj.get_orientation_as_list() + obj_rot_matrix = quaternion_matrix(obj_quat)[:3, :3] + contact_force_array = obj_rot_matrix @ np.array(contact_force).reshape(3, 1) + return contact_force_array.flatten().tolist()[2] def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj1) @@ -479,32 +523,98 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: logging.warning(f"Object {obj.name} does not exist in the simulator") - def _init_api_callback(self): + def _request_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: """ - Initialize the API callback in the request metadata. + Request the contact points of an object, this includes the object names and the contact forces and torques. + param obj: The object. + return: The contact points of the object as a list of MultiverseContactPoint. """ - self._reset_send_data() - self._reset_receive_data() - self._reset_api_callback() - self.set_simulation_in_request_meta_data() + api_response_data = self._request_apis_callbacks(self._get_contact_points_api_data(obj)) + body_names = api_response_data[self.GET_CONTACT_BODIES_API_NAME] + contact_efforts = self._parse_constraint_effort(api_response_data[self.GET_CONSTRAINT_EFFORT_API_NAME]) + return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) + for i in range(len(body_names))] - def _reset_send_data(self): + @staticmethod + def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: """ - Reset the send data in the request metadata. + Parse the contact effort of an object. + param contact_effort: The contact effort of the object as a list of strings. + return: The contact effort of the object as a list of floats. """ - self.request_meta_data["send"] = {} + return list(map(float, contact_effort[0].split())) - def _reset_receive_data(self): + def _request_objects_in_contact(self, obj: Object) -> List[str]: """ - Reset the receive data in the request metadata. + Request the objects in contact with an object. + param obj: The object. + return: The objects in contact as a list of strings for object names. """ - self.request_meta_data["receive"] = {} + return self._request_single_api_callback(self._get_contact_bodies_api_data(obj)) - def _reset_api_callback(self): + def _request_contact_effort(self, obj: Object) -> List[str]: """ - Reset the API callback in the request metadata. + Request the contact effort of an object. + param obj: The object. + return: The contact effort of the object as a list of strings that represent the contact forces and torques. """ - self.request_meta_data["api_callbacks"] = {self.simulation: []} + return self._request_single_api_callback(self._get_constraint_effort_api_data(obj)) + + def _get_contact_points_api_data(self, obj: Object) -> APIDataDict: + """ + Get the contact points API data to be added to the api callback request metadata, this includes the + contact bodies and the contact effort api data. + param obj: The object. + return: The contact points API data as an APIDataDict. + """ + api_data_list = [self._get_contact_bodies_api_data(obj), self._get_constraint_effort_api_data(obj)] + return APIDataDict.from_list(api_data_list) + + @staticmethod + def _get_contact_bodies_api_data(obj: Object): + """ + Get the contact bodies API data to be added to the api callback request metadata. + param obj: The object. + """ + return APIData("get_contact_bodies", [obj.name]) + + @staticmethod + def _get_constraint_effort_api_data(obj: Object) -> APIData: + """ + Get the constraint effort API data to be added to the api callback request metadata. + param obj: The object. + """ + return APIData("get_constraint_effort", [obj.name]) + + def _request_single_api_callback(self, api_data: APIData) -> List[str]: + """ + Request a single API callback from the server. + param api_data: The API data to request the callback. + return: The API response as a list of strings. + """ + return self._request_apis_callbacks(APIDataDict(api_data.as_dict))[api_data.api_name] + + def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: + """ + Request the API callbacks from the server. + param api_data_request: The API data to add to the request metadata. + return: The API response as a list of strings. + """ + self._init_api_callback() + for api_data, params in api_data.items(): + self._add_api_request(api_data.api_name, *api_data.params) + self._send_api_request() + return self._get_all_apis_responses() + + def _get_all_apis_responses(self) -> APIDataDict: + """ + Get all the API responses from the server. + return: The API responses as a list of APIData. + """ + list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] + dict_of_api_responses = APIDataDict({api_name: api_response for api_response in list_of_api_responses + for api_name, response in api_response.items()}) + return dict_of_api_responses def _add_api_request(self, api_name: str, *params): """ @@ -512,8 +622,6 @@ def _add_api_request(self, api_name: str, *params): param api_name: The name of the API. param params: The parameters of the API. """ - if "api_callbacks" not in self.request_meta_data: - self._init_api_callback() self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) def _send_api_request(self): @@ -525,3 +633,33 @@ def _send_api_request(self): raise ValueError self.send_and_receive_meta_data() self.request_meta_data.pop("api_callbacks") + + def _init_api_callback(self): + """ + Initialize the API callback in the request metadata. + """ + self._reset_send_data() + self._reset_receive_data() + self._reset_api_callback() + self.set_simulation_in_request_meta_data() + + def _reset_send_data(self): + """ + Reset the send data in the request metadata. + """ + self.request_meta_data["send"] = {} + + def _reset_receive_data(self): + """ + Reset the receive data in the request metadata. + """ + self.request_meta_data["receive"] = {} + + def _reset_api_callback(self): + """ + Reset the API callback in the request metadata. + """ + self.request_meta_data["api_callbacks"] = {self.simulation: []} + + def set_simulation_in_request_meta_data(self): + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation From 29abe54068aa57192102abbbc55d9cab2eca0c0f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 26 Jun 2024 18:42:56 +0200 Subject: [PATCH 036/189] [Multiverse] added check for object existence. Need to solve requirement for delay. --- src/pycram/worlds/multiverse.py | 66 +++++++++++++++++++++++++++------ test/test_multiverse.py | 45 ++++++++++++---------- 2 files changed, 79 insertions(+), 32 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 3e0a9eeb6..02db051a1 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -135,6 +135,7 @@ class Multiverse(MultiverseSocket, World): ATTACH_API_NAME = "attach" DETACH_API_NAME = "detach" GET_RAYS_API_NAME = "get_rays" + EXIST_API_NAME = "exist" """ The API names for the API callbacks to the Multiverse server. """ @@ -186,7 +187,7 @@ def _spawn_floor(self): """ floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - sleep(0.5) + # sleep(0.5) return floor def get_joint_position_name(self, joint: Joint) -> str: @@ -213,6 +214,12 @@ def load_object_and_get_id(self, name: Optional[str] = None, self.object_name_to_id[name] = self.last_object_id self.object_id_to_name[self.last_object_id] = name + while not self.check_object_exists_in_multiverse(name): + print(f"Waiting for object {name} to be loaded in Multiverse") + sleep(0.5) + + sleep(0.5) + return self.last_object_id def _init_spawn_object(self, name: str) -> None: @@ -322,9 +329,15 @@ def join_threads(self) -> None: pass def remove_object_from_simulator(self, obj: Object) -> None: + self._multiverse_remove_object(obj.name) + + def _multiverse_remove_object(self, object_name: str): + """ + Remove the object from the simulator. + """ self.set_simulation_in_request_meta_data() - self.request_meta_data["send"][obj.name] = [] - self.request_meta_data["receive"][obj.name] = [] + self.request_meta_data["send"][object_name] = [] + self.request_meta_data["receive"][object_name] = [] self.send_and_receive_meta_data() self.send_data = [time() - self.time_start] self.send_and_receive_data() @@ -348,10 +361,18 @@ def _request_attach(self, constraint: Constraint) -> None: Request to attach the child link to the parent link. param constraint: The constraint. """ + self._request_single_api_callback(self._get_attach_api_data(constraint)) + + def _get_attach_api_data(self, constraint: Constraint) -> APIData: + """ + Get the attach API data to be added to the api callback request metadata. + param constraint: The constraint. + return: The attach API data as an APIData. + """ parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - self._add_api_request("attach", child_link_name, - parent_link_name, self._get_attachment_pose_as_string(constraint)) - self._send_api_request() + return APIData(self.ATTACH_API_NAME, [child_link_name, + parent_link_name, + self._get_attachment_pose_as_string(constraint)]) def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: """ @@ -386,9 +407,16 @@ def _request_detach(self, constraint: Constraint) -> None: Request to detach the child link from the parent link. param constraint: The constraint. """ + self._request_single_api_callback(self._get_detach_api_data(constraint)) + + def _get_detach_api_data(self, constraint: Constraint) -> APIData: + """ + Get the detach API data to be added to the api callback request metadata. + param constraint: The constraint. + return: The detach API data as an APIData. + """ parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - self._add_api_request("detach", child_link_name, parent_link_name) - self._send_api_request() + return APIData(self.DETACH_API_NAME, [child_link_name, parent_link_name]) @staticmethod def get_link_name_for_constraint(link: Link) -> str: @@ -523,6 +551,17 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: logging.warning(f"Object {obj.name} does not exist in the simulator") + def check_object_exists_in_multiverse(self, object_name: str) -> bool: + result = self._request_check_object_exists(object_name)[0] + return result == "yes" + + def _request_check_object_exists(self, object_name: str) -> List[str]: + api_data = self._get_object_exists_api_data(object_name) + return self._request_single_api_callback(api_data) + + def _get_object_exists_api_data(self, object_name: str) -> APIData: + return APIData(self.EXIST_API_NAME, [object_name]) + def _request_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: """ Request the contact points of an object, this includes the object names and the contact forces and torques. @@ -592,7 +631,9 @@ def _request_single_api_callback(self, api_data: APIData) -> List[str]: param api_data: The API data to request the callback. return: The API response as a list of strings. """ - return self._request_apis_callbacks(APIDataDict(api_data.as_dict))[api_data.api_name] + api_data_dict = APIDataDict(api_data.as_dict) + response = self._request_apis_callbacks(api_data_dict) + return response[api_data.api_name] def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: """ @@ -601,8 +642,8 @@ def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: return: The API response as a list of strings. """ self._init_api_callback() - for api_data, params in api_data.items(): - self._add_api_request(api_data.api_name, *api_data.params) + for api_name, params in api_data.items(): + self._add_api_request(api_name, *params) self._send_api_request() return self._get_all_apis_responses() @@ -612,7 +653,7 @@ def _get_all_apis_responses(self) -> APIDataDict: return: The API responses as a list of APIData. """ list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] - dict_of_api_responses = APIDataDict({api_name: api_response for api_response in list_of_api_responses + dict_of_api_responses = APIDataDict({api_name: response for api_response in list_of_api_responses for api_name, response in api_response.items()}) return dict_of_api_responses @@ -632,6 +673,7 @@ def _send_api_request(self): logging.error("No API request to send") raise ValueError self.send_and_receive_meta_data() + self.send_and_receive_meta_data() self.request_meta_data.pop("api_callbacks") def _init_api_callback(self): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index cd07272a8..c45f572f6 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -52,7 +52,7 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() # self.multiverse.multiverse_reset_world() MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() - time.sleep(1) + # time.sleep(1) pass def test_reset_world(self): @@ -120,6 +120,7 @@ def test_set_joint_position(self): # time.sleep(0.1) self.assertAlmostEqual(joint_position, original_joint_position - 1.57) + # @unittest.skip("Not implemented feature yet.") def test_spawn_robot(self): if self.multiverse.robot is not None: robot = self.multiverse.robot @@ -129,6 +130,7 @@ def test_spawn_robot(self): self.assertTrue(robot in self.multiverse.objects) self.assertTrue(self.multiverse.robot.name == robot.name) + # @unittest.skip("Not implemented feature yet.") def test_destroy_robot(self): if self.multiverse.robot is None: self.spawn_robot() @@ -154,6 +156,7 @@ def test_set_robot_position(self): def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) + print(milk.get_position_as_list()) milk.attach(self.big_bowl) self.assertTrue(self.big_bowl in milk.attachments) milk_position = milk.get_position_as_list() @@ -167,21 +170,23 @@ def test_attach_object(self): self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) def test_detach_object(self): - milk = self.spawn_milk([1, 0, 0.1]) - milk.attach(self.big_bowl) - self.assertTrue(self.big_bowl in milk.attachments) - milk.detach(self.big_bowl) - self.assertTrue(self.big_bowl not in milk.attachments) - milk_position = milk.get_position_as_list() - milk_position[0] += 1 - big_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = big_bowl_position.copy() - milk.set_position(milk_position) - time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. - new_milk_position = milk.get_position_as_list() - new_bowl_position = self.big_bowl.get_position_as_list() - self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + for i in range(10): + milk = self.spawn_milk([1, 0, 0.1]) + milk.attach(self.big_bowl) + self.assertTrue(self.big_bowl in milk.attachments) + milk.detach(self.big_bowl) + self.assertTrue(self.big_bowl not in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 1 + big_bowl_position = self.big_bowl.get_position_as_list() + estimated_bowl_position = big_bowl_position.copy() + milk.set_position(milk_position) + # time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. + new_milk_position = milk.get_position_as_list() + new_bowl_position = self.big_bowl.get_position_as_list() + self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) + self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + self.tearDown() @unittest.skip("Needs mobile base robot") def test_attach_with_robot(self): @@ -208,7 +213,7 @@ def test_get_object_contact_points(self): self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) big_bowl_position = self.big_bowl.get_position_as_list() milk.set_position([big_bowl_position[0], big_bowl_position[1], big_bowl_position[2] + 0.5]) - time.sleep(0.5) + # time.sleep(0.5) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) @@ -218,14 +223,14 @@ def test_get_object_contact_points(self): def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) - time.sleep(0.5) + # time.sleep(0.5) return big_bowl @staticmethod def spawn_milk(position: List) -> Object: milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(0.5) + # time.sleep(0.5) return milk @staticmethod @@ -234,5 +239,5 @@ def spawn_robot(position: Optional[List[float]] = None) -> Object: position = [0, 0, 0.1] robot = Object("panda", ObjectType.ROBOT, "panda.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(1) + # time.sleep(1) return robot From 7a1627c3f482a5ad18879bbdecf1db564e457de4 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 2 Jul 2024 02:43:43 +0200 Subject: [PATCH 037/189] [Contact] added robots in data_directories. --- src/pycram/datastructures/world.py | 16 +++++++++++----- test/test_multiverse.py | 3 ++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 8108abab3..51c06f374 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -19,7 +19,7 @@ from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState, ContactPoint, ClosestPoint, ClosestPointsList, + ObjectState, State, WorldState, ClosestPointsList, ContactPointsList) from ..datastructures.enums import JointType, ObjectType, WorldMode from ..datastructures.pose import Pose, Transform @@ -129,13 +129,19 @@ class World(StateEntity, ABC): the URDF with the name of the URDF on the parameter server. """ - data_directory: List[str] = [os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources')] + resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') """ - Global reference for the data directories, this is used to search for the description files of the robot - and the objects. + Global reference for the resources path, this is used to search for the description files of the robot and + the objects. """ - cache_dir: str = data_directory[0] + '/cached/' + data_directory: List[str] = [resources_path, os.path.join(resources_path, 'robots')] + """ + Global reference for the data directories, this is used to search for the description files of the robot, + the objects, and the cached files. + """ + + cache_dir: str = os.path.join(resources_path, 'cached') """ Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index c45f572f6..4040c8e6b 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -165,7 +165,7 @@ def test_attach_object(self): estimated_bowl_position = big_bowl_position.copy() estimated_bowl_position[0] += 1 milk.set_position(milk_position) - time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. + time.sleep(1) # TODO: This is a workaround for the issue that the position is not updated immediately. new_bowl_position = self.big_bowl.get_position_as_list() self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) @@ -207,6 +207,7 @@ def test_attach_with_robot(self): def test_get_object_contact_points(self): milk = self.spawn_milk([1, 0, 0.1]) + time.sleep(1) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) From 2b6b992286b3b407ed4f454990b96a002d1b8655 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 3 Jul 2024 17:16:18 +0200 Subject: [PATCH 038/189] [Multiverse] fixing some multiverse issues. --- .../world_concepts/multiverse_socket.py | 1 + src/pycram/worlds/multiverse.py | 48 ++++++++----------- test/test_multiverse.py | 20 ++++---- 3 files changed, 31 insertions(+), 38 deletions(-) diff --git a/src/pycram/world_concepts/multiverse_socket.py b/src/pycram/world_concepts/multiverse_socket.py index 66f327bd4..55e615b27 100644 --- a/src/pycram/world_concepts/multiverse_socket.py +++ b/src/pycram/world_concepts/multiverse_socket.py @@ -44,6 +44,7 @@ def __init__( self._send_data = None self._client_addr = client_addr self._meta_data = multiverse_meta_data + self.client_name = self._meta_data.simulation_name self._multiverse_socket = MultiverseClientPybind( f"{self._server_addr.host}:{self._server_addr.port}" ) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 02db051a1..8ed0de323 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -214,12 +214,6 @@ def load_object_and_get_id(self, name: Optional[str] = None, self.object_name_to_id[name] = self.last_object_id self.object_id_to_name[self.last_object_id] = name - while not self.check_object_exists_in_multiverse(name): - print(f"Waiting for object {name} to be loaded in Multiverse") - sleep(0.5) - - sleep(0.5) - return self.last_object_id def _init_spawn_object(self, name: str) -> None: @@ -239,9 +233,7 @@ def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] def _init_getter(self): - self.request_meta_data["receive"] = {} - self.request_meta_data["send"] = {} - self.request_meta_data["meta_data"]["simulation_name"] = self._meta_data.simulation_name + self._reset_request_meta_data() def get_joint_position(self, joint: Joint) -> float: self.check_object_exists_and_issue_warning_if_not(joint.object) @@ -256,9 +248,8 @@ def get_joint_position(self, joint: Joint) -> float: return receive_data[0] def _init_setter(self): - self.request_meta_data["send"] = {} - self.request_meta_data["receive"] = {} - self.set_simulation_in_request_meta_data() + self._reset_request_meta_data() + self._set_simulation_in_request_meta_data() def reset_joint_position(self, joint: Joint, joint_position: float) -> None: self._init_setter() @@ -323,6 +314,7 @@ def get_all_objects_data_from_server(self) -> Dict[str, Dict]: return self.response_meta_data["receive"] def disconnect_from_physics_server(self) -> None: + self._reset_request_meta_data() self.stop() def join_threads(self) -> None: @@ -335,7 +327,7 @@ def _multiverse_remove_object(self, object_name: str): """ Remove the object from the simulator. """ - self.set_simulation_in_request_meta_data() + self._set_simulation_in_request_meta_data() self.request_meta_data["send"][object_name] = [] self.request_meta_data["receive"][object_name] = [] self.send_and_receive_meta_data() @@ -673,35 +665,33 @@ def _send_api_request(self): logging.error("No API request to send") raise ValueError self.send_and_receive_meta_data() - self.send_and_receive_meta_data() self.request_meta_data.pop("api_callbacks") def _init_api_callback(self): """ Initialize the API callback in the request metadata. """ - self._reset_send_data() - self._reset_receive_data() + self._reset_request_meta_data() self._reset_api_callback() - self.set_simulation_in_request_meta_data() - def _reset_send_data(self): + def _reset_api_callback(self): """ - Reset the send data in the request metadata. + Reset the API callback in the request metadata. """ - self.request_meta_data["send"] = {} + self.request_meta_data["api_callbacks"] = {self.simulation: []} - def _reset_receive_data(self): + def _set_simulation_in_request_meta_data(self): """ - Reset the receive data in the request metadata. + Set the simulation name in the request metadata. (for e.g. name of simulator in the muv file) """ - self.request_meta_data["receive"] = {} + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation - def _reset_api_callback(self): + def _reset_request_meta_data(self): """ - Reset the API callback in the request metadata. + Reset the request metadata. """ - self.request_meta_data["api_callbacks"] = {self.simulation: []} - - def set_simulation_in_request_meta_data(self): - self.request_meta_data["meta_data"]["simulation_name"] = self.simulation + self.request_meta_data = { + "meta_data": self._meta_data.__dict__, + "send": {}, + "receive": {}, + } \ No newline at end of file diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4040c8e6b..b9392154e 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -49,11 +49,10 @@ def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() def tearDown(self): - self.multiverse.reset_world_and_remove_objects() - # self.multiverse.multiverse_reset_world() + # self.multiverse.reset_world_and_remove_objects() + self.multiverse.multiverse_reset_world() + self.multiverse._reset_request_meta_data() MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() - # time.sleep(1) - pass def test_reset_world(self): set_position = [1, 1, 0.1] @@ -81,6 +80,7 @@ def test_remove_object(self): milk = self.spawn_milk([0, 0, 0.1]) milk.remove() + @unittest.skip("Not implemented feature yet.") def test_check_object_exists(self): milk = self.spawn_milk([0, 0, 0.1]) data = self.multiverse.get_all_objects_data_from_server() @@ -154,6 +154,7 @@ def test_set_robot_position(self): self.multiverse.robot.set_position(new_position) self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) + # @unittest.skip("Not implemented feature yet.") def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) print(milk.get_position_as_list()) @@ -165,10 +166,11 @@ def test_attach_object(self): estimated_bowl_position = big_bowl_position.copy() estimated_bowl_position[0] += 1 milk.set_position(milk_position) - time.sleep(1) # TODO: This is a workaround for the issue that the position is not updated immediately. + time.sleep(0.1) new_bowl_position = self.big_bowl.get_position_as_list() self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + @unittest.skip("Not implemented feature yet.") def test_detach_object(self): for i in range(10): milk = self.spawn_milk([1, 0, 0.1]) @@ -207,7 +209,7 @@ def test_attach_with_robot(self): def test_get_object_contact_points(self): milk = self.spawn_milk([1, 0, 0.1]) - time.sleep(1) + # time.sleep(1) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) @@ -224,14 +226,14 @@ def test_get_object_contact_points(self): def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) - # time.sleep(0.5) + time.sleep(0.5) return big_bowl @staticmethod def spawn_milk(position: List) -> Object: milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", pose=Pose(position, [0, 0, 0, 1])) - # time.sleep(0.5) + time.sleep(0.5) return milk @staticmethod @@ -240,5 +242,5 @@ def spawn_robot(position: Optional[List[float]] = None) -> Object: position = [0, 0, 0.1] robot = Object("panda", ObjectType.ROBOT, "panda.urdf", pose=Pose(position, [0, 0, 0, 1])) - # time.sleep(1) + time.sleep(1) return robot From 96bda065c57e404bef964cda548f68bd4957416d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 3 Jul 2024 17:29:34 +0200 Subject: [PATCH 039/189] [Multiverse] merge with pycram dev. --- test/test_multiverse.py | 1 - 1 file changed, 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b9392154e..0b44ccd20 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -157,7 +157,6 @@ def test_set_robot_position(self): # @unittest.skip("Not implemented feature yet.") def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) - print(milk.get_position_as_list()) milk.attach(self.big_bowl) self.assertTrue(self.big_bowl in milk.attachments) milk_position = milk.get_position_as_list() From 5936fb172c70222867905ead9c6d22c279c6e2ed Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 3 Jul 2024 17:38:30 +0200 Subject: [PATCH 040/189] [CacheManager] corrected cache path for object. --- src/pycram/cache_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 1c7b86cea..7737be58d 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -46,7 +46,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, self.create_cache_dir_if_not_exists() # save correct path in case the file is already in the cache directory - cache_path = self.cache_dir + object_description.get_file_name(path_object, extension, object_name) + cache_path = os.path.join(self.cache_dir, object_description.get_file_name(path_object, extension, object_name)) if not self.is_cached(path, object_description) or ignore_cached_files: # if file is not yet cached preprocess the description file and save it in the cache directory. From b680b6e20f94eb171be6c675f4a937b66efdf708 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 4 Jul 2024 20:05:15 +0200 Subject: [PATCH 041/189] [Multiverse] some tests have been fixed, still need to test more. --- src/pycram/worlds/multiverse.py | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 8ed0de323..ec838f7d0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,11 +1,11 @@ import logging import os from dataclasses import dataclass -from time import time, sleep +from time import time -from typing_extensions import List, Dict, Optional, Tuple -from tf.transformations import quaternion_matrix import numpy as np +from tf.transformations import quaternion_matrix +from typing_extensions import List, Dict, Optional, Tuple from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -96,6 +96,7 @@ class APIDataDict(dict): """ A dictionary to store the API data, where the key is the API name and the value is the parameters. """ + @classmethod def from_list(cls, api_data_list: List[APIData]) -> "APIDataDict": data = {api_data.api_name: api_data.params for api_data in api_data_list} @@ -237,8 +238,9 @@ def _init_getter(self): def get_joint_position(self, joint: Joint) -> float: self.check_object_exists_and_issue_warning_if_not(joint.object) - self._init_getter() attribute = self.get_joint_position_name(joint) + self._check_for_get(joint.name, [attribute]) + self._init_getter() self.request_meta_data["receive"][joint.name] = [attribute] self.send_and_receive_meta_data() receive_data = self.response_meta_data["receive"][joint.name][attribute] @@ -267,7 +269,17 @@ def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) return self._get_body_pose(obj.name) + def _check_for_get(self, object_name: str, object_props: List[str]) -> None: + while True: + self._init_getter() + self.request_meta_data["receive"][""] = [""] + self.send_and_receive_meta_data() + if (object_name in self.response_meta_data["receive"] and + all([prop in self.response_meta_data["receive"][object_name] for prop in object_props])): + break + def _get_body_pose(self, body_name: str) -> Pose: + self._check_for_get(body_name, ["position", "quaternion"]) self._init_getter() self.request_meta_data["receive"][body_name] = ["position", "quaternion"] self.send_and_receive_meta_data() @@ -691,7 +703,7 @@ def _reset_request_meta_data(self): Reset the request metadata. """ self.request_meta_data = { - "meta_data": self._meta_data.__dict__, + "meta_data": self._meta_data.__dict__.copy(), "send": {}, "receive": {}, - } \ No newline at end of file + } From 392815ae9375889c7143cb313a5c2b7e67698856 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 9 Jul 2024 23:00:55 +0200 Subject: [PATCH 042/189] [Multiverse] Testing. --- src/pycram/datastructures/world.py | 3 +- src/pycram/worlds/multiverse.py | 7 +- test/test_multiverse.py | 164 +++++++++++++++++------------ 3 files changed, 103 insertions(+), 71 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 51c06f374..59de11fc8 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -864,7 +864,8 @@ def reset_world_and_remove_objects(self) -> None: """ self.reset_world() for obj in copy(self.objects): - self.remove_object(obj) + if obj.name != 'floor': + self.remove_object(obj) def reset_world(self, remove_saved_states=True) -> None: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ec838f7d0..5ca3e065f 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -297,7 +297,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self._reset_body_pose(obj.name, pose) def multiverse_reset_world(self): - self._init_setter() + self._reset_request_meta_data() self.send_and_receive_meta_data() self.send_data = [0] self.send_and_receive_data() @@ -352,7 +352,8 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError - self._request_attach(constraint) + if not self.set_attached_objects_poses: + self._request_attach(constraint) self.last_constraint_id += 1 @@ -553,7 +554,7 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: - logging.warning(f"Object {obj.name} does not exist in the simulator") + logging.warning(f"Object {obj} does not exist in the simulator") def check_object_exists_in_multiverse(self, object_name: str) -> bool: result = self._request_check_object_exists(object_name)[0] diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 0b44ccd20..e017bb005 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -7,7 +7,6 @@ from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose -from pycram.datastructures.world import UseProspectionWorld from pycram.world_concepts.world_object import Object multiverse_installed = True @@ -42,45 +41,45 @@ def setUpClass(cls): cls.multiverse = Multiverse(simulation="pycram_test", client_addr=SocketAddress(port="5481"), is_prospection=True) - cls.big_bowl = cls.spawn_big_bowl() + # cls.big_bowl = cls.spawn_big_bowl() @classmethod def tearDownClass(cls): cls.multiverse.disconnect_from_physics_server() def tearDown(self): - # self.multiverse.reset_world_and_remove_objects() - self.multiverse.multiverse_reset_world() - self.multiverse._reset_request_meta_data() - MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() + # self.multiverse.multiverse_reset_world() + self.multiverse.reset_world_and_remove_objects() + # MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() def test_reset_world(self): set_position = [1, 1, 0.1] - self.big_bowl.set_position(set_position) - bowl_position = self.big_bowl.get_position_as_list() - for i in range(3): - self.assertAlmostEqual(bowl_position[i], set_position[i]) + milk = self.spawn_milk(set_position) + milk.set_position(set_position) + time.sleep(0.1) + milk_position = milk.get_position_as_list() + self.assert_list_is_equal(milk_position[:2], set_position[:2]) self.multiverse.reset_world() - big_bowl_pose = self.big_bowl.get_pose() - for v1, v2 in zip(big_bowl_pose.position_as_list()[:2], self.big_bowl.original_pose.position_as_list()[:2]): - self.assertAlmostEqual(v1, v2, delta=0.001) - for v1, v2 in zip(big_bowl_pose.orientation_as_list(), self.big_bowl.original_pose.orientation_as_list()): - self.assertAlmostEqual(v1, v2, delta=0.001) + time.sleep(0.1) + milk_pose = milk.get_pose() + self.assert_list_is_equal(milk_pose.position_as_list()[:2], + milk.original_pose.position_as_list()[:2]) + self.assert_list_is_equal(milk_pose.orientation_as_list(), + milk.original_pose.orientation_as_list()) def test_spawn_object(self): milk = self.spawn_milk([1, 0, 0.1]) self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() - for v1, v2 in zip(milk_pose.position_as_list()[:2], [1, 0]): - self.assertAlmostEqual(v1, v2, delta=0.002) - for v1, v2 in zip(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()): - self.assertAlmostEqual(v1, v2, delta=0.002) + self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 0]) + self.assert_list_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_remove_object(self): milk = self.spawn_milk([0, 0, 0.1]) milk.remove() + self.assertTrue(milk not in self.multiverse.objects) + self.assertFalse(self.multiverse.check_object_exists_in_multiverse(milk.name)) - @unittest.skip("Not implemented feature yet.") def test_check_object_exists(self): milk = self.spawn_milk([0, 0, 0.1]) data = self.multiverse.get_all_objects_data_from_server() @@ -92,15 +91,13 @@ def test_set_position(self): original_milk_position[0] += 1 milk.set_position(original_milk_position) milk_position = milk.get_position_as_list() - for v1, v2 in zip(milk_position[:2], original_milk_position[:2]): - self.assertAlmostEqual(v1, v2, delta=0.002) + self.assert_list_is_equal(milk_position[:2], original_milk_position[:2]) def test_update_position(self): milk = self.spawn_milk([1, 0, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() - for i, v in enumerate([1, 0]): - self.assertAlmostEqual(milk_position[i], v, delta=0.002) + self.assert_list_is_equal(milk_position[:2], [1, 0], delta=0.002) def test_set_joint_position(self): if self.multiverse.robot is None: @@ -118,7 +115,7 @@ def test_set_joint_position(self): break i += 1 # time.sleep(0.1) - self.assertAlmostEqual(joint_position, original_joint_position - 1.57) + self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.02) # @unittest.skip("Not implemented feature yet.") def test_spawn_robot(self): @@ -148,64 +145,65 @@ def test_respawn_robot(self): self.assertTrue(self.multiverse.robot in self.multiverse.objects) def test_set_robot_position(self): - if self.multiverse.robot is None: - self.spawn_robot() + self.spawn_mobile_robot(robot_name='panda_free') new_position = [1, 1, 0.1] self.multiverse.robot.set_position(new_position) - self.assertEqual(self.multiverse.robot.get_position_as_list(), new_position) + robot_position = self.multiverse.robot.get_position_as_list() + self.assert_positon_is_equal(robot_position, new_position, delta=0.035) - # @unittest.skip("Not implemented feature yet.") def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) - milk.attach(self.big_bowl) - self.assertTrue(self.big_bowl in milk.attachments) + cup = self.spawn_cup([1, 1, 0.1]) + milk.attach(cup) + self.assertTrue(cup in milk.attachments) milk_position = milk.get_position_as_list() milk_position[0] += 1 - big_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = big_bowl_position.copy() - estimated_bowl_position[0] += 1 + cup_position = cup.get_position_as_list() + estimated_cup_position = cup_position.copy() + estimated_cup_position[0] += 1 milk.set_position(milk_position) time.sleep(0.1) - new_bowl_position = self.big_bowl.get_position_as_list() - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + new_cup_position = cup.get_position_as_list() + self.assertAlmostEqual(new_cup_position[0], estimated_cup_position[0], delta=0.001) - @unittest.skip("Not implemented feature yet.") + # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): for i in range(10): milk = self.spawn_milk([1, 0, 0.1]) - milk.attach(self.big_bowl) - self.assertTrue(self.big_bowl in milk.attachments) - milk.detach(self.big_bowl) - self.assertTrue(self.big_bowl not in milk.attachments) + cup = self.spawn_cup([1, 1, 0.1]) + milk.attach(cup) + self.assertTrue(cup in milk.attachments) + milk.detach(cup) + self.assertTrue(cup not in milk.attachments) milk_position = milk.get_position_as_list() milk_position[0] += 1 - big_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = big_bowl_position.copy() + cup_position = cup.get_position_as_list() + estimated_cup_position = cup_position.copy() milk.set_position(milk_position) # time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. new_milk_position = milk.get_position_as_list() - new_bowl_position = self.big_bowl.get_position_as_list() + new_cup_position = cup.get_position_as_list() self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0], delta=0.001) + self.assertAlmostEqual(new_cup_position[0], estimated_cup_position[0], delta=0.001) + cup.remove() + milk.remove() self.tearDown() - @unittest.skip("Needs mobile base robot") def test_attach_with_robot(self): - with UseProspectionWorld(): - if self.multiverse.robot is None: - robot = self.spawn_robot() - robot.attach(self.big_bowl) - self.assertTrue(self.big_bowl in robot.attachments) - bowl_position = self.big_bowl.get_position_as_list() - robot.update_pose() - robot_position = robot.get_position_as_list() - robot_position[2] += 3 - robot.set_position(robot_position) - new_bowl_position = self.big_bowl.get_position_as_list() - estimated_bowl_position = bowl_position - estimated_bowl_position[2] += 3 - self.assertAlmostEqual(new_bowl_position[0], estimated_bowl_position[0]) + milk = self.spawn_milk([1, 0, 0.1]) + robot = self.spawn_robot() + # Get position of milk relative to robot end effector + milk_initial_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) + robot.attach(milk, robot.tip_link_name) + self.assertTrue(robot in milk.attachments) + robot_position = robot.get_joint_position("joint1") + robot_position += 1.57 + robot.set_joint_position("joint1", robot_position) + time.sleep(0.1) + milk_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) + self.assert_poses_are_equal(milk_initial_pose, milk_pose) + @unittest.skip("Not implemented feature yet.") def test_get_object_contact_points(self): milk = self.spawn_milk([1, 0, 0.1]) # time.sleep(1) @@ -225,21 +223,53 @@ def test_get_object_contact_points(self): def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) - time.sleep(0.5) + # time.sleep(0.1) return big_bowl @staticmethod def spawn_milk(position: List) -> Object: milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", pose=Pose(position, [0, 0, 0, 1])) - time.sleep(0.5) + # time.sleep(0.5) return milk - @staticmethod - def spawn_robot(position: Optional[List[float]] = None) -> Object: + def spawn_mobile_robot(self, position: Optional[List[float]] = None, robot_name: Optional[str] = 'tiago_dual') -> Object: + self.spawn_robot(position, robot_name, replace=True) + + def spawn_robot(self, position: Optional[List[float]] = None, + robot_name: Optional[str] = 'panda', + replace: Optional[bool] = False) -> Object: if position is None: position = [0, 0, 0.1] - robot = Object("panda", ObjectType.ROBOT, "panda.urdf", - pose=Pose(position, [0, 0, 0, 1])) - time.sleep(1) + if self.multiverse.robot is None or replace: + if self.multiverse.robot is not None: + self.multiverse.robot.remove() + robot = Object(robot_name, ObjectType.ROBOT, f"{robot_name}.urdf", + pose=Pose(position, [0, 0, 0, 1])) + else: + robot = self.multiverse.robot + robot.set_position(position) + # time.sleep(0.5) return robot + + @staticmethod + def spawn_cup(position: List) -> Object: + cup = Object("cup", ObjectType.GENERIC_OBJECT, "Cup.obj", + pose=Pose(position, [0, 0, 0, 1])) + # time.sleep(0.5) + return cup + + def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, + position_delta: float = 0.02, orientation_delta: float = 0.01): + self.assert_positon_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) + self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) + + def assert_positon_is_equal(self, position1: List[float], position2: List[float], delta: float = 0.02): + self.assert_list_is_equal(position1, position2, delta=delta) + + def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], delta: float = 0.01): + self.assert_list_is_equal(orientation1, orientation2, delta=delta) + + def assert_list_is_equal(self, list1: List, list2: List, delta: float = 0.02): + for i in range(len(list1)): + self.assertAlmostEqual(list1[i], list2[i], delta=delta) From 71bc2620f5b19c399eb7a5f233d35ea7bb207c99 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 10 Jul 2024 20:05:34 +0200 Subject: [PATCH 043/189] [Multiverse] Multi client new structure. --- .../world_concepts/multiverse_clients.py | 370 ++++++++++++++ .../world_concepts/multiverse_socket.py | 4 +- src/pycram/worlds/multiverse.py | 468 ++++-------------- test/test_multiverse.py | 7 +- 4 files changed, 458 insertions(+), 391 deletions(-) create mode 100644 src/pycram/world_concepts/multiverse_clients.py diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py new file mode 100644 index 000000000..00b47cf9d --- /dev/null +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -0,0 +1,370 @@ +import logging +import threading +from dataclasses import dataclass +from time import time + +from typing_extensions import List, Dict, Tuple, Optional + +from .constraints import Constraint +from .multiverse_socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from .world_object import Object, Link +from ..datastructures.pose import Pose + + +@dataclass +class APIData: + """ + A dataclass to store the API data. + """ + api_name: str + params: List[str] + + @property + def as_dict(self) -> Dict: + return {self.api_name: self.params} + + +class APIDataDict(dict): + """ + A dictionary to store the API data, where the key is the API name and the value is the parameters. + """ + + @classmethod + def from_list(cls, api_data_list: List[APIData]) -> "APIDataDict": + data = {api_data.api_name: api_data.params for api_data in api_data_list} + return cls(data) + + +@dataclass +class MultiverseContactPoint: + """ + A dataclass to store the contact point returned from Multiverse. + """ + body_name: str + contact_force: List[float] + contact_torque: List[float] + + +class MultiverseReader(MultiverseSocket): + def __init__(self): + meta_data = MultiverseMetaData() + meta_data.simulation_name = "reader" + super().__init__(SocketAddress(port="8000"), meta_data) + self.run() + self.data_lock = threading.Lock() + self.thread = threading.Thread(target=self.get_all_data_from_server) + self.stop_thread = False + self.request_meta_data["receive"][""] = [""] + self.thread.start() + + def get_all_data_from_server(self): + """ + Get all data from the multiverse server. + """ + while not self.stop_thread: + self.data_lock.acquire() + self.send_and_receive_meta_data() + self.data_lock.release() + self.stop() + + def join(self): + self.thread.join() + + +class MultiverseWriter(MultiverseSocket): + def __init__(self, simulation: str): + meta_data = MultiverseMetaData() + meta_data.simulation_name = "writer" + super().__init__(SocketAddress(port="8001"), meta_data) + self.simulation = simulation + self.time_start = time() + self.run() + + def _reset_request_meta_data(self): + """ + Reset the request metadata. + """ + self.request_meta_data = { + "meta_data": self._meta_data.__dict__.copy(), + "send": {}, + "receive": {}, + } + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation + + def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]): + self.send_body_data_to_server(body_name, + {"position": position, + "orientation": orientation}) + + def set_body_position(self, body_name: str, position: List[float]): + self.send_body_data_to_server(body_name, {"position": position}) + + def set_body_orientation(self, body_name: str, orientation: List[float]): + self.send_body_data_to_server(body_name, {"orientation": orientation}) + + def remove_body(self, body_name: str): + self.send_data_to_server([time() - self.time_start], + send_meta_data={body_name: []}, + receive_meta_data={body_name: []}) + + def reset_world(self): + self.send_data_to_server([0]) + + def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) -> Dict: + """ + Send data to the multiverse server. + param body_name: The name of the body. + param data: The data to be sent. + """ + send_meta_data = {body_name: list(data.keys())} + send_data = [time() - self.time_start, *data.values()] + return self.send_data_to_server(send_data, send_meta_data=send_meta_data) + + def send_data_to_server(self, data: List, + send_meta_data: Optional[Dict] = None, + receive_meta_data: Optional[Dict] = None) -> Dict: + """ + Send data to the multiverse server. + param data: The data to be sent. + param send_meta_data: The metadata to be sent. + """ + self._reset_request_meta_data() + if send_meta_data: + self.request_meta_data["send"] = send_meta_data + if receive_meta_data: + self.request_meta_data["receive"] = receive_meta_data + self.send_and_receive_meta_data() + self.send_data = data + self.send_and_receive_data() + return self.response_meta_data + + +class MultiverseAPI(MultiverseSocket): + + GET_CONTACT_BODIES_API_NAME = "get_contact_bodies" + GET_CONSTRAINT_EFFORT_API_NAME = "get_constraint_effort" + ATTACH_API_NAME = "attach" + DETACH_API_NAME = "detach" + GET_RAYS_API_NAME = "get_rays" + EXIST_API_NAME = "exist" + """ + The API names for the API callbacks to the Multiverse server. + """ + + def __init__(self, simulation: str): + meta_data = MultiverseMetaData() + meta_data.simulation_name = "api_requester" + super().__init__(SocketAddress(port="8002"), meta_data) + self.simulation = simulation + + def attach(self, constraint: Constraint) -> None: + """ + Request to attach the child link to the parent link. + param constraint: The constraint. + """ + self._request_single_api_callback(self._get_attach_api_data(constraint)) + + def _get_attach_api_data(self, constraint: Constraint) -> APIData: + """ + Get the attach API data to be added to the api callback request metadata. + param constraint: The constraint. + return: The attach API data as an APIData. + """ + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + return APIData(self.ATTACH_API_NAME, [child_link_name, + parent_link_name, + self._get_attachment_pose_as_string(constraint)]) + + def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: + """ + Get the link names of the constraint. + param constraint: The constraint. + return: The link names of the constraint. + """ + return self.get_parent_link_name(constraint), self.get_constraint_child_link_name(constraint) + + def get_parent_link_name(self, constraint: Constraint) -> str: + """ + Get the parent link name of the constraint. + param constraint: The constraint. + return: The parent link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.parent_link) + + def get_constraint_child_link_name(self, constraint: Constraint) -> str: + """ + Get the child link name of the constraint. + param constraint: The constraint. + return: The child link name of the constraint. + """ + return self.get_link_name_for_constraint(constraint.child_link) + + @staticmethod + def get_link_name_for_constraint(link: Link) -> str: + """ + Get the link name from link object, if the link belongs to a one link object, return the object name. + param link: The link. + return: The link name. + """ + return link.name if not link.is_only_link else link.object.name + + def detach(self, constraint: Constraint) -> None: + """ + Request to detach the child link from the parent link. + param constraint: The constraint. + """ + self._request_single_api_callback(self._get_detach_api_data(constraint)) + + def _get_detach_api_data(self, constraint: Constraint) -> APIData: + """ + Get the detach API data to be added to the api callback request metadata. + param constraint: The constraint. + return: The detach API data as an APIData. + """ + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + return APIData(self.DETACH_API_NAME, [child_link_name, parent_link_name]) + + def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: + """ + Get the attachment pose as a string. + param constraint: The constraint. + return: The attachment pose as a string. + """ + pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) + return self._pose_to_string(pose) + + @staticmethod + def _pose_to_string(pose: Pose) -> str: + """ + Convert the pose to a string. + param pose: The pose. + return: The pose as a string. + """ + return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ + f"{pose.orientation.y} {pose.orientation.z}" + + def check_object_exists(self, object_name: str) -> List[str]: + api_data = self._get_object_exists_api_data(object_name) + return self._request_single_api_callback(api_data) + + def _get_object_exists_api_data(self, object_name: str) -> APIData: + return APIData(self.EXIST_API_NAME, [object_name]) + + def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: + """ + Request the contact points of an object, this includes the object names and the contact forces and torques. + param obj: The object. + return: The contact points of the object as a list of MultiverseContactPoint. + """ + api_response_data = self._request_apis_callbacks(self._get_contact_points_api_data(obj)) + body_names = api_response_data[self.GET_CONTACT_BODIES_API_NAME] + contact_efforts = self._parse_constraint_effort(api_response_data[self.GET_CONSTRAINT_EFFORT_API_NAME]) + return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) + for i in range(len(body_names))] + + @staticmethod + def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: + """ + Parse the contact effort of an object. + param contact_effort: The contact effort of the object as a list of strings. + return: The contact effort of the object as a list of floats. + """ + return list(map(float, contact_effort[0].split())) + + def _request_objects_in_contact(self, obj: Object) -> List[str]: + """ + Request the objects in contact with an object. + param obj: The object. + return: The objects in contact as a list of strings for object names. + """ + return self._request_single_api_callback(self._get_contact_bodies_api_data(obj)) + + def _request_contact_effort(self, obj: Object) -> List[str]: + """ + Request the contact effort of an object. + param obj: The object. + return: The contact effort of the object as a list of strings that represent the contact forces and torques. + """ + return self._request_single_api_callback(self._get_constraint_effort_api_data(obj)) + + def _get_contact_points_api_data(self, obj: Object) -> APIDataDict: + """ + Get the contact points API data to be added to the api callback request metadata, this includes the + contact bodies and the contact effort api data. + param obj: The object. + return: The contact points API data as an APIDataDict. + """ + api_data_list = [self._get_contact_bodies_api_data(obj), self._get_constraint_effort_api_data(obj)] + return APIDataDict.from_list(api_data_list) + + @staticmethod + def _get_contact_bodies_api_data(obj: Object): + """ + Get the contact bodies API data to be added to the api callback request metadata. + param obj: The object. + """ + return APIData("get_contact_bodies", [obj.name]) + + @staticmethod + def _get_constraint_effort_api_data(obj: Object) -> APIData: + """ + Get the constraint effort API data to be added to the api callback request metadata. + param obj: The object. + """ + return APIData("get_constraint_effort", [obj.name]) + + def _request_single_api_callback(self, api_data: APIData) -> List[str]: + """ + Request a single API callback from the server. + param api_data: The API data to request the callback. + return: The API response as a list of strings. + """ + api_data_dict = APIDataDict(api_data.as_dict) + response = self._request_apis_callbacks(api_data_dict) + return response[api_data.api_name] + + def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: + """ + Request the API callbacks from the server. + param api_data_request: The API data to add to the request metadata. + return: The API response as a list of strings. + """ + self._reset_api_callback() + for api_name, params in api_data.items(): + self._add_api_request(api_name, *params) + self._send_api_request() + return self._get_all_apis_responses() + + def _get_all_apis_responses(self) -> APIDataDict: + """ + Get all the API responses from the server. + return: The API responses as a list of APIData. + """ + list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] + dict_of_api_responses = APIDataDict({api_name: response for api_response in list_of_api_responses + for api_name, response in api_response.items()}) + return dict_of_api_responses + + def _add_api_request(self, api_name: str, *params): + """ + Add an API request to the request metadata. + param api_name: The name of the API. + param params: The parameters of the API. + """ + self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) + + def _send_api_request(self): + """ + Send the API request to the server. + """ + if "api_callbacks" not in self.request_meta_data: + logging.error("No API request to send") + raise ValueError + self.send_and_receive_meta_data() + self.request_meta_data.pop("api_callbacks") + + def _reset_api_callback(self): + """ + Initialize the API callback in the request metadata. + """ + self.request_meta_data["api_callbacks"] = {self.simulation: []} diff --git a/src/pycram/world_concepts/multiverse_socket.py b/src/pycram/world_concepts/multiverse_socket.py index 55e615b27..7353e95d6 100644 --- a/src/pycram/world_concepts/multiverse_socket.py +++ b/src/pycram/world_concepts/multiverse_socket.py @@ -5,6 +5,7 @@ import logging from multiverse_client_pybind import MultiverseClientPybind # noqa +from typing_extensions import Optional T = TypeVar("T") @@ -30,9 +31,6 @@ def __init__(self, port: str) -> None: class MultiverseSocket: _server_addr: SocketAddress = SocketAddress(port="7000") - _client_addr: SocketAddress - _meta_data: MultiverseMetaData - _multiverse_socket: MultiverseClientPybind def __init__( self, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5ca3e065f..a6dfdcb20 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,11 +1,9 @@ import logging import os -from dataclasses import dataclass -from time import time import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Tuple +from typing_extensions import List, Dict, Optional from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -13,7 +11,7 @@ from ..datastructures.world import World from ..description import Link, Joint from ..world_concepts.constraints import Constraint -from ..world_concepts.multiverse_socket import MultiverseSocket, SocketAddress +from ..world_concepts.multiverse_clients import MultiverseReader, MultiverseWriter, MultiverseAPI from ..world_concepts.world_object import Object @@ -79,41 +77,7 @@ def find_multiverse_path() -> Optional[str]: return None -@dataclass -class APIData: - """ - A dataclass to store the API data. - """ - api_name: str - params: List[str] - - @property - def as_dict(self) -> Dict: - return {self.api_name: self.params} - - -class APIDataDict(dict): - """ - A dictionary to store the API data, where the key is the API name and the value is the parameters. - """ - - @classmethod - def from_list(cls, api_data_list: List[APIData]) -> "APIDataDict": - data = {api_data.api_name: api_data.params for api_data in api_data_list} - return cls(data) - - -@dataclass -class MultiverseContactPoint: - """ - A dataclass to store the contact point returned from Multiverse. - """ - body_name: str - contact_force: List[float] - contact_torque: List[float] - - -class Multiverse(MultiverseSocket, World): +class Multiverse(World): """ This class implements an interface between Multiverse and PyCRAM. """ @@ -131,20 +95,9 @@ class Multiverse(MultiverseSocket, World): A flag to check if the multiverse resources have been added. """ - GET_CONTACT_BODIES_API_NAME = "get_contact_bodies" - GET_CONSTRAINT_EFFORT_API_NAME = "get_constraint_effort" - ATTACH_API_NAME = "attach" - DETACH_API_NAME = "detach" - GET_RAYS_API_NAME = "get_rays" - EXIST_API_NAME = "exist" - """ - The API names for the API callbacks to the Multiverse server. - """ - def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, - simulation_frequency: Optional[float] = 60.0, - client_addr: Optional[SocketAddress] = SocketAddress(port="7000")): + simulation_frequency: Optional[float] = 60.0): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -152,21 +105,34 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. """ - MultiverseSocket.__init__(self, client_addr) World.__init__(self, mode, is_prospection, simulation_frequency) - self.simulation: str = simulation + + self._init_clients(simulation) + self._make_sure_multiverse_resources_are_added() + + self._set_world_job_flags() + + self._init_constraint_and_object_id_name_map_collections() + + self._spawn_floor() + + def _init_clients(self, simulation: str): + self.reader = MultiverseReader() + self.writer = MultiverseWriter(simulation) + self.api_requester = MultiverseAPI(simulation) + + def _set_world_job_flags(self): self.set_attached_objects_poses = False self.handle_spawning = False self.update_poses_on_get = True + + def _init_constraint_and_object_id_name_map_collections(self): self.last_object_id: int = -1 self.last_constraint_id: int = -1 self.constraints: Dict[int, Constraint] = {} self.object_name_to_id: Dict[str, int] = {} self.object_id_to_name: Dict[int, str] = {} - self.time_start = time() - self.run() - self.floor = self._spawn_floor() def _init_world(self, mode: WorldMode): pass @@ -186,10 +152,8 @@ def _spawn_floor(self): """ Spawn the plane in the simulator. """ - floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", - world=self) - # sleep(0.5) - return floor + self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", + world=self) def get_joint_position_name(self, joint: Joint) -> str: if joint.type not in self._joint_type_to_position_name: @@ -210,21 +174,19 @@ def load_object_and_get_id(self, name: Optional[str] = None, self._reset_body_pose(name, pose) - self.last_object_id += 1 + return self._update_object_id_name_maps_and_get_latest_id(name) + def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: + """ + Update the object id name maps and return the latest object id. + param name: The name of the object. + return: The latest object id. + """ + self.last_object_id += 1 self.object_name_to_id[name] = self.last_object_id self.object_id_to_name[self.last_object_id] = name - return self.last_object_id - def _init_spawn_object(self, name: str) -> None: - """ - Initialize the object spawning process. - """ - self._init_setter() - self.request_meta_data["send"][name] = ["position", "quaternion"] - self.send_and_receive_meta_data() - def get_object_joint_names(self, obj: Object) -> List[str]: return [joint.name for joint in obj.description.joints @@ -233,33 +195,25 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] - def _init_getter(self): - self._reset_request_meta_data() - def get_joint_position(self, joint: Joint) -> float: self.check_object_exists_and_issue_warning_if_not(joint.object) attribute = self.get_joint_position_name(joint) - self._check_for_get(joint.name, [attribute]) - self._init_getter() - self.request_meta_data["receive"][joint.name] = [attribute] - self.send_and_receive_meta_data() - receive_data = self.response_meta_data["receive"][joint.name][attribute] - if len(receive_data) != 1: - logging.error(f"Invalid joint position data: {receive_data}") - raise ValueError - return receive_data[0] + received_data = self._check_for_get(joint.name, [attribute]) + attribute_value = received_data[joint.name][attribute] + return attribute_value[0] - def _init_setter(self): - self._reset_request_meta_data() - self._set_simulation_in_request_meta_data() + def get_data_from_reader(self) -> Dict: + self.reader.data_lock.acquire() + received_objects = self.reader.response_meta_data['receive'] + self.reader.data_lock.release() + return received_objects def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - self._init_setter() attribute = self.get_joint_position_name(joint) - self.request_meta_data["send"][joint.name] = [attribute] - self.send_and_receive_meta_data() - self.send_data = [time() - self.time_start, joint_position] - self.send_and_receive_data() + self.send_data_to_server(joint.name, {attribute: [joint_position]}) + + def send_data_to_server(self, body_name: str, data: Dict[str, List[float]]): + self.writer.send_body_data_to_server(body_name, data) def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) @@ -269,38 +223,33 @@ def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) return self._get_body_pose(obj.name) - def _check_for_get(self, object_name: str, object_props: List[str]) -> None: + def _check_for_get(self, object_name: str, object_props: List[str]) -> Dict: while True: - self._init_getter() - self.request_meta_data["receive"][""] = [""] - self.send_and_receive_meta_data() - if (object_name in self.response_meta_data["receive"] and - all([prop in self.response_meta_data["receive"][object_name] for prop in object_props])): - break + received_data = self.get_data_from_reader() + if (object_name in received_data and + all([prop in received_data[object_name] for prop in object_props])): + return received_data def _get_body_pose(self, body_name: str) -> Pose: - self._check_for_get(body_name, ["position", "quaternion"]) - self._init_getter() - self.request_meta_data["receive"][body_name] = ["position", "quaternion"] - self.send_and_receive_meta_data() - self.send_data = [time() - self.time_start] - self.send_and_receive_data() - if len(self.receive_data) != 8: - logging.error(f"Invalid body pose data: {self.receive_data}") - raise ValueError - wxyz = self.receive_data[4:] + """ + Get the pose of a body in the simulator. + param body_name: The name of the body. + return: The pose of the body. + """ + if body_name == "floor": + return Pose() + received_data = self._check_for_get(body_name, ["position", "quaternion"]) + body_data = received_data[body_name] + wxyz = body_data["quaternion"] xyzw = [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] - return Pose(self.receive_data[1:4], xyzw) + return Pose(body_data["position"], xyzw) def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) self._reset_body_pose(obj.name, pose) def multiverse_reset_world(self): - self._reset_request_meta_data() - self.send_and_receive_meta_data() - self.send_data = [0] - self.send_and_receive_data() + self.writer.reset_world() def _reset_body_pose(self, body_name: str, pose: Pose) -> None: """ @@ -308,43 +257,22 @@ def _reset_body_pose(self, body_name: str, pose: Pose) -> None: param body_name: The name of the body. param pose: The pose of the body. """ - self._init_setter() - self.request_meta_data["send"][body_name] = ["position", "quaternion"] - self.send_and_receive_meta_data() xyzw = pose.orientation_as_list() wxyz = [xyzw[3], *xyzw[:3]] - self.send_data = [time() - self.time_start, *pose.position_as_list(), *wxyz] - self.send_and_receive_data() - - def get_all_objects_data_from_server(self) -> Dict[str, Dict]: - """ - Get all objects data from the multiverse server. - """ - self._init_getter() - self.request_meta_data["receive"][""] = [""] - self.send_and_receive_meta_data() - return self.response_meta_data["receive"] + self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) def disconnect_from_physics_server(self) -> None: - self._reset_request_meta_data() - self.stop() + self.writer.stop() + self.api_requester.stop() + self.reader.stop_thread = True + self.reader.join() def join_threads(self) -> None: - pass + self.reader.stop_thread = True + self.reader.join() def remove_object_from_simulator(self, obj: Object) -> None: - self._multiverse_remove_object(obj.name) - - def _multiverse_remove_object(self, object_name: str): - """ - Remove the object from the simulator. - """ - self._set_simulation_in_request_meta_data() - self.request_meta_data["send"][object_name] = [] - self.request_meta_data["receive"][object_name] = [] - self.send_and_receive_meta_data() - self.send_data = [time() - self.time_start] - self.send_and_receive_data() + self.writer.remove_body(obj.name) def add_constraint(self, constraint: Constraint) -> int: @@ -352,113 +280,34 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError - if not self.set_attached_objects_poses: - self._request_attach(constraint) - - self.last_constraint_id += 1 - - self.constraints[self.last_constraint_id] = constraint - - return self.last_constraint_id - - def _request_attach(self, constraint: Constraint) -> None: - """ - Request to attach the child link to the parent link. - param constraint: The constraint. - """ - self._request_single_api_callback(self._get_attach_api_data(constraint)) - - def _get_attach_api_data(self, constraint: Constraint) -> APIData: - """ - Get the attach API data to be added to the api callback request metadata. - param constraint: The constraint. - return: The attach API data as an APIData. - """ - parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - return APIData(self.ATTACH_API_NAME, [child_link_name, - parent_link_name, - self._get_attachment_pose_as_string(constraint)]) + self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) + self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) - def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: - """ - Get the link names of the constraint. - param constraint: The constraint. - return: The link names of the constraint. - """ - return self.get_parent_link_name(constraint), self.get_constraint_child_link_name(constraint) + if not self.set_attached_objects_poses: + self.api_requester.attach(constraint) - def get_parent_link_name(self, constraint: Constraint) -> str: - """ - Get the parent link name of the constraint. - param constraint: The constraint. - return: The parent link name of the constraint. - """ - return self.get_link_name_for_constraint(constraint.parent_link) + return self._update_constraint_collection_and_get_latest_id(constraint) - def get_constraint_child_link_name(self, constraint: Constraint) -> str: + def _update_constraint_collection_and_get_latest_id(self, constraint: Constraint) -> int: """ - Get the child link name of the constraint. - param constraint: The constraint. - return: The child link name of the constraint. + Update the constraint collection and return the latest constraint id. + param constraint: The constraint to be added. + return: The latest constraint id. """ - return self.get_link_name_for_constraint(constraint.child_link) + self.last_constraint_id += 1 + self.constraints[self.last_constraint_id] = constraint + return self.last_constraint_id def remove_constraint(self, constraint_id) -> None: constraint = self.constraints.pop(constraint_id) - self._request_detach(constraint) - - def _request_detach(self, constraint: Constraint) -> None: - """ - Request to detach the child link from the parent link. - param constraint: The constraint. - """ - self._request_single_api_callback(self._get_detach_api_data(constraint)) - - def _get_detach_api_data(self, constraint: Constraint) -> APIData: - """ - Get the detach API data to be added to the api callback request metadata. - param constraint: The constraint. - return: The detach API data as an APIData. - """ - parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - return APIData(self.DETACH_API_NAME, [child_link_name, parent_link_name]) - - @staticmethod - def get_link_name_for_constraint(link: Link) -> str: - """ - Get the link name from link object, if the link belongs to a one link object, return the object name. - param link: The link. - return: The link name. - """ - return link.name if not link.is_only_link else link.object.name - - def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: - """ - Get the attachment pose as a string. - param constraint: The constraint. - return: The attachment pose as a string. - """ - self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) - self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) - pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) - return self._pose_to_string(pose) - - @staticmethod - def _pose_to_string(pose: Pose) -> str: - """ - Convert the pose to a string. - param pose: The pose. - return: The pose as a string. - """ - return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ - f"{pose.orientation.y} {pose.orientation.z}" + self.api_requester.detach(constraint) def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") def get_object_contact_points(self, obj: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj) - multiverse_contact_points = self._request_contact_points(obj) + multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None for point in multiverse_contact_points: @@ -557,154 +406,5 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): logging.warning(f"Object {obj} does not exist in the simulator") def check_object_exists_in_multiverse(self, object_name: str) -> bool: - result = self._request_check_object_exists(object_name)[0] + result = self.api_requester.check_object_exists(object_name)[0] return result == "yes" - - def _request_check_object_exists(self, object_name: str) -> List[str]: - api_data = self._get_object_exists_api_data(object_name) - return self._request_single_api_callback(api_data) - - def _get_object_exists_api_data(self, object_name: str) -> APIData: - return APIData(self.EXIST_API_NAME, [object_name]) - - def _request_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: - """ - Request the contact points of an object, this includes the object names and the contact forces and torques. - param obj: The object. - return: The contact points of the object as a list of MultiverseContactPoint. - """ - api_response_data = self._request_apis_callbacks(self._get_contact_points_api_data(obj)) - body_names = api_response_data[self.GET_CONTACT_BODIES_API_NAME] - contact_efforts = self._parse_constraint_effort(api_response_data[self.GET_CONSTRAINT_EFFORT_API_NAME]) - return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) - for i in range(len(body_names))] - - @staticmethod - def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: - """ - Parse the contact effort of an object. - param contact_effort: The contact effort of the object as a list of strings. - return: The contact effort of the object as a list of floats. - """ - return list(map(float, contact_effort[0].split())) - - def _request_objects_in_contact(self, obj: Object) -> List[str]: - """ - Request the objects in contact with an object. - param obj: The object. - return: The objects in contact as a list of strings for object names. - """ - return self._request_single_api_callback(self._get_contact_bodies_api_data(obj)) - - def _request_contact_effort(self, obj: Object) -> List[str]: - """ - Request the contact effort of an object. - param obj: The object. - return: The contact effort of the object as a list of strings that represent the contact forces and torques. - """ - return self._request_single_api_callback(self._get_constraint_effort_api_data(obj)) - - def _get_contact_points_api_data(self, obj: Object) -> APIDataDict: - """ - Get the contact points API data to be added to the api callback request metadata, this includes the - contact bodies and the contact effort api data. - param obj: The object. - return: The contact points API data as an APIDataDict. - """ - api_data_list = [self._get_contact_bodies_api_data(obj), self._get_constraint_effort_api_data(obj)] - return APIDataDict.from_list(api_data_list) - - @staticmethod - def _get_contact_bodies_api_data(obj: Object): - """ - Get the contact bodies API data to be added to the api callback request metadata. - param obj: The object. - """ - return APIData("get_contact_bodies", [obj.name]) - - @staticmethod - def _get_constraint_effort_api_data(obj: Object) -> APIData: - """ - Get the constraint effort API data to be added to the api callback request metadata. - param obj: The object. - """ - return APIData("get_constraint_effort", [obj.name]) - - def _request_single_api_callback(self, api_data: APIData) -> List[str]: - """ - Request a single API callback from the server. - param api_data: The API data to request the callback. - return: The API response as a list of strings. - """ - api_data_dict = APIDataDict(api_data.as_dict) - response = self._request_apis_callbacks(api_data_dict) - return response[api_data.api_name] - - def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: - """ - Request the API callbacks from the server. - param api_data_request: The API data to add to the request metadata. - return: The API response as a list of strings. - """ - self._init_api_callback() - for api_name, params in api_data.items(): - self._add_api_request(api_name, *params) - self._send_api_request() - return self._get_all_apis_responses() - - def _get_all_apis_responses(self) -> APIDataDict: - """ - Get all the API responses from the server. - return: The API responses as a list of APIData. - """ - list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] - dict_of_api_responses = APIDataDict({api_name: response for api_response in list_of_api_responses - for api_name, response in api_response.items()}) - return dict_of_api_responses - - def _add_api_request(self, api_name: str, *params): - """ - Add an API request to the request metadata. - param api_name: The name of the API. - param params: The parameters of the API. - """ - self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) - - def _send_api_request(self): - """ - Send the API request to the server. - """ - if "api_callbacks" not in self.request_meta_data: - logging.error("No API request to send") - raise ValueError - self.send_and_receive_meta_data() - self.request_meta_data.pop("api_callbacks") - - def _init_api_callback(self): - """ - Initialize the API callback in the request metadata. - """ - self._reset_request_meta_data() - self._reset_api_callback() - - def _reset_api_callback(self): - """ - Reset the API callback in the request metadata. - """ - self.request_meta_data["api_callbacks"] = {self.simulation: []} - - def _set_simulation_in_request_meta_data(self): - """ - Set the simulation name in the request metadata. (for e.g. name of simulator in the muv file) - """ - self.request_meta_data["meta_data"]["simulation_name"] = self.simulation - - def _reset_request_meta_data(self): - """ - Reset the request metadata. - """ - self.request_meta_data = { - "meta_data": self._meta_data.__dict__.copy(), - "send": {}, - "receive": {}, - } diff --git a/test/test_multiverse.py b/test/test_multiverse.py index e017bb005..1a94de005 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -39,7 +39,6 @@ def setUpClass(cls): if not multiverse_installed: return cls.multiverse = Multiverse(simulation="pycram_test", - client_addr=SocketAddress(port="5481"), is_prospection=True) # cls.big_bowl = cls.spawn_big_bowl() @@ -82,8 +81,7 @@ def test_remove_object(self): def test_check_object_exists(self): milk = self.spawn_milk([0, 0, 0.1]) - data = self.multiverse.get_all_objects_data_from_server() - self.assertTrue(milk.name in data) + self.assertTrue(self.multiverse.check_object_exists_in_multiverse(milk.name)) def test_set_position(self): milk = self.spawn_milk([0, 0, 0.1]) @@ -144,6 +142,7 @@ def test_respawn_robot(self): self.spawn_robot(position=[0, 0, 0.1]) self.assertTrue(self.multiverse.robot in self.multiverse.objects) + @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): self.spawn_mobile_robot(robot_name='panda_free') new_position = [1, 1, 0.1] @@ -260,7 +259,7 @@ def spawn_cup(position: List) -> Object: return cup def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, - position_delta: float = 0.02, orientation_delta: float = 0.01): + position_delta: float = 0.02, orientation_delta: float = 0.02): self.assert_positon_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) From 4137a98b5a3af52ccb03cb81f477ed4c40c671eb Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 11 Jul 2024 18:01:46 +0200 Subject: [PATCH 044/189] [Multiverse] Tests are running after restructure. --- src/pycram/datastructures/world.py | 6 +- src/pycram/description.py | 4 +- .../world_concepts/multiverse_clients.py | 200 ++++++++++++++++-- src/pycram/world_concepts/world_object.py | 6 +- src/pycram/worlds/multiverse.py | 45 ++-- test/test_multiverse.py | 12 +- 6 files changed, 212 insertions(+), 61 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 59de11fc8..c6fbb325d 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -186,9 +186,9 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_events() - self.set_attached_objects_poses = True - self.handle_spawning = True - self.update_poses_on_get = False + self.let_pycram_move_attached_objects = True + self.let_pycram_handle_spawning = True + self.update_poses_from_sim_on_get = False self._current_state: Optional[WorldState] = None diff --git a/src/pycram/description.py b/src/pycram/description.py index fc1b19c0e..bdec4fafa 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -349,7 +349,7 @@ def pose(self) -> Pose: :return: A Pose object containing the pose of the link relative to the world frame. """ - if self.world.update_poses_on_get: + if self.world.update_poses_from_sim_on_get: self._update_pose() return self._current_pose @@ -487,7 +487,7 @@ def child_link(self) -> Link: @property def position(self) -> float: - if self.world.update_poses_on_get: + if self.world.update_poses_from_sim_on_get: self._update_position() return self._current_position diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index 00b47cf9d..f14f70efe 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -1,7 +1,7 @@ import logging import threading from dataclasses import dataclass -from time import time +from time import time, sleep from typing_extensions import List, Dict, Tuple, Optional @@ -46,25 +46,137 @@ class MultiverseContactPoint: class MultiverseReader(MultiverseSocket): - def __init__(self): + def __init__(self, max_wait_time_for_data: Optional[float] = 1): + """ + Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. + This class provides methods to get data (e.g., position, orientation) from the Multiverse server. + param max_wait_time_for_data: The maximum wait time for the data in seconds. + """ + self.max_wait_time_for_data = max_wait_time_for_data meta_data = MultiverseMetaData() meta_data.simulation_name = "reader" super().__init__(SocketAddress(port="8000"), meta_data) self.run() self.data_lock = threading.Lock() - self.thread = threading.Thread(target=self.get_all_data_from_server) + self.thread = threading.Thread(target=self.receive_all_data_from_server) self.stop_thread = False self.request_meta_data["receive"][""] = [""] self.thread.start() - def get_all_data_from_server(self): + def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose]: + """ + Get the body pose from the multiverse server. + param name: The name of the body. + param wait: Whether to wait for the data. + return: The position and orientation of the body. + """ + data = self.get_body_data(name, ["position", "quaternion"], wait=wait) + if data is not None: + return Pose(data["position"], self.wxyz_to_xyzw(data["quaternion"])) + + def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + """ + Get the body position from the multiverse server. + param name: The name of the body. + param wait: Whether to wait for the data. + return: The position of the body. + """ + return self.get_body_data(name, ["position"], wait=wait) + + def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + """ + Get the body orientation from the multiverse server. + param name: The name of the body. + param wait: Whether to wait for the data. + return: The orientation of the body. + """ + data = self.get_body_property(name, "quaternion", wait=wait) + if data is not None: + return self.wxyz_to_xyzw(data) + + def get_body_property(self, name: str, property_name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + """ + Get the body property from the multiverse server. + param name: The name of the body. + param property_name: The name of the property. + param wait: Whether to wait for the data. + return: The property of the body. + """ + data = self.get_body_data(name, [property_name], wait=wait) + if data is not None: + return data[property_name] + + @staticmethod + def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: + """ + Convert the quaternion from wxyz to xyzw. + param quaternion: The quaternion as a list of floats. + return: The quaternion as a list of floats. + """ + return quaternion[1:] + [quaternion[0]] + + def get_body_data(self, name: str, + properties: Optional[List[str]] = None, + wait: Optional[bool] = True) -> Optional[Dict]: + """ + Get the body data from the multiverse server. + param name: The name of the body. + param properties: The properties of the body. + param wait: Whether to wait for the data. + return: The body data as a dictionary. + """ + if wait: + return self.wait_for_body_data(name, properties) + elif self.check_for_body_data(name, self.get_received_data(), properties): + return self.get_received_data()[name] + + def wait_for_body_data(self, name: str, properties: Optional[List[str]] = None) -> Dict: + """ + Wait for the body data from the multiverse server. + param name: The name of the body. + param properties: The properties of the body. + return: The body data as a dictionary. + """ + start = time() + while time() - start < self.max_wait_time_for_data: + received_data = self.get_received_data() + data_received_flag = self.check_for_body_data(name, received_data, properties) + if data_received_flag: + return received_data[name] + + @staticmethod + def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = None) -> bool: + """ + Check if the body data is received from the multiverse server. + param name: The name of the body. + param data: The data received from the multiverse server. + param properties: The properties of the body. + return: Whether the body data is received. + """ + if properties is None: + return name in data + else: + return name in data and all([prop in data[name] and None not in data[name][prop] for prop in properties]) + + def get_received_data(self): + """ + Get the latest received data from the multiverse server. + """ + self.data_lock.acquire() + data = self.response_meta_data["receive"] + self.data_lock.release() + return data + + def receive_all_data_from_server(self): """ Get all data from the multiverse server. """ while not self.stop_thread: + self.request_meta_data["receive"][""] = [""] self.data_lock.acquire() self.send_and_receive_meta_data() self.data_lock.release() + sleep(0.01) self.stop() def join(self): @@ -72,7 +184,19 @@ def join(self): class MultiverseWriter(MultiverseSocket): + + time_for_sim_update: Optional[float] = 0.3 + """ + Wait time for the sent data to be applied in the simulator. + """ + def __init__(self, simulation: str): + """ + Initialize the Multiverse writer, which writes the data to the Multiverse server. + This class provides methods to send data (e.g., position, orientation) to the Multiverse server. + param simulation: The name of the simulation that the writer is connected to + (usually the name defined in the .muv file). + """ meta_data = MultiverseMetaData() meta_data.simulation_name = "writer" super().__init__(SocketAddress(port="8001"), meta_data) @@ -91,23 +215,46 @@ def _reset_request_meta_data(self): } self.request_meta_data["meta_data"]["simulation_name"] = self.simulation - def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]): + def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]) -> None: + """ + Set the body pose in the simulation. + param body_name: The name of the body. + param position: The position of the body. + param orientation: The orientation of the body. + """ self.send_body_data_to_server(body_name, {"position": position, - "orientation": orientation}) + "quaternion": orientation}) - def set_body_position(self, body_name: str, position: List[float]): + def set_body_position(self, body_name: str, position: List[float]) -> None: + """ + Set the body position in the simulation. + param body_name: The name of the body. + param position: The position of the body. + """ self.send_body_data_to_server(body_name, {"position": position}) - def set_body_orientation(self, body_name: str, orientation: List[float]): - self.send_body_data_to_server(body_name, {"orientation": orientation}) + def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: + """ + Set the body orientation in the simulation. + param body_name: The name of the body. + param orientation: The orientation of the body. + """ + self.send_body_data_to_server(body_name, {"quaternion": orientation}) - def remove_body(self, body_name: str): + def remove_body(self, body_name: str) -> None: + """ + Remove the body from the simulation. + param body_name: The name of the body. + """ self.send_data_to_server([time() - self.time_start], send_meta_data={body_name: []}, receive_meta_data={body_name: []}) - def reset_world(self): + def reset_world(self) -> None: + """ + Reset the world in the simulation. + """ self.send_data_to_server([0]) def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) -> Dict: @@ -115,10 +262,14 @@ def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) Send data to the multiverse server. param body_name: The name of the body. param data: The data to be sent. + return: The response from the server. """ send_meta_data = {body_name: list(data.keys())} - send_data = [time() - self.time_start, *data.values()] - return self.send_data_to_server(send_data, send_meta_data=send_meta_data) + flattened_data = [item for sublist in data.values() for item in sublist] + send_data = [time() - self.time_start, *flattened_data] + response = self.send_data_to_server(send_data, send_meta_data=send_meta_data) + sleep(self.time_for_sim_update) + return response def send_data_to_server(self, data: List, send_meta_data: Optional[Dict] = None, @@ -127,6 +278,8 @@ def send_data_to_server(self, data: List, Send data to the multiverse server. param data: The data to be sent. param send_meta_data: The metadata to be sent. + param receive_meta_data: The metadata to be received. + return: The response from the server. """ self._reset_request_meta_data() if send_meta_data: @@ -152,10 +305,17 @@ class MultiverseAPI(MultiverseSocket): """ def __init__(self, simulation: str): + """ + Initialize the Multiverse API, which sends API requests to the Multiverse server. + This class provides methods like attach and detach objects, get contact points, and other API requests. + param simulation: The name of the simulation that the API is connected to + (usually the name defined in the .muv file). + """ meta_data = MultiverseMetaData() meta_data.simulation_name = "api_requester" super().__init__(SocketAddress(port="8002"), meta_data) self.simulation = simulation + self.run() def attach(self, constraint: Constraint) -> None: """ @@ -243,11 +403,21 @@ def _pose_to_string(pose: Pose) -> str: return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ f"{pose.orientation.y} {pose.orientation.z}" - def check_object_exists(self, object_name: str) -> List[str]: + def check_object_exists(self, object_name: str) -> bool: + """ + Check if the object exists in the simulation. + param object_name: The name of the object. + return: Whether the object exists in the simulation. + """ api_data = self._get_object_exists_api_data(object_name) - return self._request_single_api_callback(api_data) + return self._request_single_api_callback(api_data)[0] == 'yes' def _get_object_exists_api_data(self, object_name: str) -> APIData: + """ + Get the object exists API data to be added to the api callback request metadata. + param object_name: The name of the object. + return: The object exists API data as an APIData. + """ return APIData(self.EXIST_API_NAME, [object_name]) def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index b4fb6e9bf..75c3e5641 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -134,7 +134,7 @@ def _load_object_and_get_id(self, path: str, raise e try: - path = self.path if self.world.handle_spawning else self.name + path = self.path if self.world.let_pycram_handle_spawning else self.name obj_id = self.world.load_object_and_get_id(path, self._current_pose) return obj_id, self.path @@ -488,7 +488,7 @@ def get_pose(self) -> Pose: :return: The current pose of this object """ - if self.world.update_poses_on_get: + if self.world.update_poses_from_sim_on_get: self.update_pose() return self._current_pose @@ -666,7 +666,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - if not self.world.set_attached_objects_poses: + if not self.world.let_pycram_move_attached_objects: return if already_moved_objects is None: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a6dfdcb20..4589acc9e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -123,9 +123,9 @@ def _init_clients(self, simulation: str): self.api_requester = MultiverseAPI(simulation) def _set_world_job_flags(self): - self.set_attached_objects_poses = False - self.handle_spawning = False - self.update_poses_on_get = True + self.let_pycram_move_attached_objects = False + self.let_pycram_handle_spawning = False + self.update_poses_from_sim_on_get = True def _init_constraint_and_object_id_name_map_collections(self): self.last_object_id: int = -1 @@ -172,7 +172,7 @@ def load_object_and_get_id(self, name: Optional[str] = None, if pose is None: pose = Pose() - self._reset_body_pose(name, pose) + self._set_body_pose(name, pose) return self._update_object_id_name_maps_and_get_latest_id(name) @@ -197,16 +197,11 @@ def get_object_link_names(self, obj: Object) -> List[str]: def get_joint_position(self, joint: Joint) -> float: self.check_object_exists_and_issue_warning_if_not(joint.object) - attribute = self.get_joint_position_name(joint) - received_data = self._check_for_get(joint.name, [attribute]) - attribute_value = received_data[joint.name][attribute] - return attribute_value[0] + property_name = self.get_joint_position_name(joint) + return self.get_body_property(joint.name, property_name) - def get_data_from_reader(self) -> Dict: - self.reader.data_lock.acquire() - received_objects = self.reader.response_meta_data['receive'] - self.reader.data_lock.release() - return received_objects + def get_body_property(self, body_name: str, property_name: str) -> float: + return self.reader.get_body_property(body_name, property_name)[0] def reset_joint_position(self, joint: Joint, joint_position: float) -> None: attribute = self.get_joint_position_name(joint) @@ -223,13 +218,6 @@ def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) return self._get_body_pose(obj.name) - def _check_for_get(self, object_name: str, object_props: List[str]) -> Dict: - while True: - received_data = self.get_data_from_reader() - if (object_name in received_data and - all([prop in received_data[object_name] for prop in object_props])): - return received_data - def _get_body_pose(self, body_name: str) -> Pose: """ Get the pose of a body in the simulator. @@ -238,22 +226,18 @@ def _get_body_pose(self, body_name: str) -> Pose: """ if body_name == "floor": return Pose() - received_data = self._check_for_get(body_name, ["position", "quaternion"]) - body_data = received_data[body_name] - wxyz = body_data["quaternion"] - xyzw = [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] - return Pose(body_data["position"], xyzw) + return self.reader.get_body_pose(body_name) def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) - self._reset_body_pose(obj.name, pose) + self._set_body_pose(obj.name, pose) def multiverse_reset_world(self): self.writer.reset_world() - def _reset_body_pose(self, body_name: str, pose: Pose) -> None: + def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ - Reset the pose of a body in the simulator. + Reset the pose of a body (object, link, or joint) in the simulator. param body_name: The name of the body. param pose: The pose of the body. """ @@ -283,7 +267,7 @@ def add_constraint(self, constraint: Constraint) -> int: self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) - if not self.set_attached_objects_poses: + if not self.let_pycram_move_attached_objects: self.api_requester.attach(constraint) return self._update_constraint_collection_and_get_latest_id(constraint) @@ -406,5 +390,4 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): logging.warning(f"Object {obj} does not exist in the simulator") def check_object_exists_in_multiverse(self, object_name: str) -> bool: - result = self.api_requester.check_object_exists(object_name)[0] - return result == "yes" + return self.api_requester.check_object_exists(object_name) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 1a94de005..239bf47c2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -44,7 +44,7 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): - cls.multiverse.disconnect_from_physics_server() + cls.multiverse.exit() def tearDown(self): # self.multiverse.multiverse_reset_world() @@ -55,11 +55,9 @@ def test_reset_world(self): set_position = [1, 1, 0.1] milk = self.spawn_milk(set_position) milk.set_position(set_position) - time.sleep(0.1) milk_position = milk.get_position_as_list() self.assert_list_is_equal(milk_position[:2], set_position[:2]) self.multiverse.reset_world() - time.sleep(0.1) milk_pose = milk.get_pose() self.assert_list_is_equal(milk_pose.position_as_list()[:2], milk.original_pose.position_as_list()[:2]) @@ -161,13 +159,13 @@ def test_attach_object(self): estimated_cup_position = cup_position.copy() estimated_cup_position[0] += 1 milk.set_position(milk_position) - time.sleep(0.1) + # time.sleep(0.3) new_cup_position = cup.get_position_as_list() self.assertAlmostEqual(new_cup_position[0], estimated_cup_position[0], delta=0.001) # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): - for i in range(10): + for i in range(1): milk = self.spawn_milk([1, 0, 0.1]) cup = self.spawn_cup([1, 1, 0.1]) milk.attach(cup) @@ -179,7 +177,7 @@ def test_detach_object(self): cup_position = cup.get_position_as_list() estimated_cup_position = cup_position.copy() milk.set_position(milk_position) - # time.sleep(0.1) # TODO: This is a workaround for the issue that the position is not updated immediately. + # time.sleep(0.3) # TODO: This is a workaround for the issue that the position is not updated immediately. new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) @@ -198,7 +196,7 @@ def test_attach_with_robot(self): robot_position = robot.get_joint_position("joint1") robot_position += 1.57 robot.set_joint_position("joint1", robot_position) - time.sleep(0.1) + # time.sleep(0.1) milk_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) From f0d5f18eee8175f7aaf37e953a7bd97b38a8de12 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 11 Jul 2024 19:00:28 +0200 Subject: [PATCH 045/189] [Multiverse] Tests are running after restructure and more accurate. --- test/test_multiverse.py | 35 +++++++++++++---------------------- 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 239bf47c2..ca6aa682a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -93,7 +93,7 @@ def test_update_position(self): milk = self.spawn_milk([1, 0, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], [1, 0], delta=0.002) + self.assert_list_is_equal(milk_position[:2], [1, 0]) def test_set_joint_position(self): if self.multiverse.robot is None: @@ -111,9 +111,8 @@ def test_set_joint_position(self): break i += 1 # time.sleep(0.1) - self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.02) + self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.001) - # @unittest.skip("Not implemented feature yet.") def test_spawn_robot(self): if self.multiverse.robot is not None: robot = self.multiverse.robot @@ -131,7 +130,6 @@ def test_destroy_robot(self): self.multiverse.robot.remove() self.assertTrue(self.multiverse.robot not in self.multiverse.objects) - @unittest.skip("Not implemented feature yet.") def test_respawn_robot(self): self.spawn_robot() self.assertTrue(self.multiverse.robot in self.multiverse.objects) @@ -140,7 +138,7 @@ def test_respawn_robot(self): self.spawn_robot(position=[0, 0, 0.1]) self.assertTrue(self.multiverse.robot in self.multiverse.objects) - @unittest.skip("This will cause respawning of the robot.") + # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): self.spawn_mobile_robot(robot_name='panda_free') new_position = [1, 1, 0.1] @@ -159,13 +157,12 @@ def test_attach_object(self): estimated_cup_position = cup_position.copy() estimated_cup_position[0] += 1 milk.set_position(milk_position) - # time.sleep(0.3) new_cup_position = cup.get_position_as_list() - self.assertAlmostEqual(new_cup_position[0], estimated_cup_position[0], delta=0.001) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2]) # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): - for i in range(1): + for i in range(2): milk = self.spawn_milk([1, 0, 0.1]) cup = self.spawn_cup([1, 1, 0.1]) milk.attach(cup) @@ -177,13 +174,10 @@ def test_detach_object(self): cup_position = cup.get_position_as_list() estimated_cup_position = cup_position.copy() milk.set_position(milk_position) - # time.sleep(0.3) # TODO: This is a workaround for the issue that the position is not updated immediately. new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assertAlmostEqual(new_milk_position[0], milk_position[0], delta=0.001) - self.assertAlmostEqual(new_cup_position[0], estimated_cup_position[0], delta=0.001) - cup.remove() - milk.remove() + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.002) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) self.tearDown() def test_attach_with_robot(self): @@ -196,25 +190,22 @@ def test_attach_with_robot(self): robot_position = robot.get_joint_position("joint1") robot_position += 1.57 robot.set_joint_position("joint1", robot_position) - # time.sleep(0.1) milk_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) - @unittest.skip("Not implemented feature yet.") def test_get_object_contact_points(self): milk = self.spawn_milk([1, 0, 0.1]) - # time.sleep(1) + cup = self.spawn_cup([1, 1, 0.1]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) - big_bowl_position = self.big_bowl.get_position_as_list() - milk.set_position([big_bowl_position[0], big_bowl_position[1], big_bowl_position[2] + 0.5]) - # time.sleep(0.5) + cup_position = cup.get_position_as_list() + milk.set_position([cup_position[0], cup_position[1], cup_position[2] + 0.2]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) - self.assertTrue(contact_points[0].link_b.object, self.big_bowl) + self.assertTrue(contact_points[0].link_b.object, cup) @staticmethod def spawn_big_bowl() -> Object: @@ -257,7 +248,7 @@ def spawn_cup(position: List) -> Object: return cup def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, - position_delta: float = 0.02, orientation_delta: float = 0.02): + position_delta: float = 0.002, orientation_delta: float = 0.002): self.assert_positon_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) @@ -267,6 +258,6 @@ def assert_positon_is_equal(self, position1: List[float], position2: List[float] def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], delta: float = 0.01): self.assert_list_is_equal(orientation1, orientation2, delta=delta) - def assert_list_is_equal(self, list1: List, list2: List, delta: float = 0.02): + def assert_list_is_equal(self, list1: List, list2: List, delta: float = 0.001): for i in range(len(list1)): self.assertAlmostEqual(list1[i], list2[i], delta=delta) From 90bd09775913f4d49d7409251bc556d9603111fd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 11 Jul 2024 19:56:21 +0200 Subject: [PATCH 046/189] [Multiverse] Tests are running with contact. --- src/pycram/datastructures/dataclasses.py | 3 +++ .../world_concepts/multiverse_clients.py | 2 +- src/pycram/worlds/multiverse.py | 12 ++++++--- test/test_multiverse.py | 25 ++++++++----------- 4 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 701941e86..ff28053a9 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -315,6 +315,9 @@ class ContactPoint: normal_force: Optional[List[float]] = None # normal force applied during last step simulation lateral_friction_1: Optional[LateralFriction] = None lateral_friction_2: Optional[LateralFriction] = None + force_x_in_world_frame: Optional[float] = None + force_y_in_world_frame: Optional[float] = None + force_z_in_world_frame: Optional[float] = None def __str__(self): return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}" diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index f14f70efe..30a1cdfe6 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -185,7 +185,7 @@ def join(self): class MultiverseWriter(MultiverseSocket): - time_for_sim_update: Optional[float] = 0.3 + time_for_sim_update: Optional[float] = 0.4 """ Wait time for the sent data to be applied in the simulator. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 4589acc9e..726e42858 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -310,9 +310,13 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: logging.error(f"Body link not found: {point.body_name}") raise ValueError contact_points.append(ContactPoint(obj.root_link, body_link)) - normal_force = self._get_normal_force_on_object_from_contact_force(obj, point.contact_force) - contact_points[-1].normal_on_b = normal_force - contact_points[-1].normal_force = normal_force + b_obj = body_link.object + # normal_force_in_b_frame = self._get_normal_force_on_object_from_contact_force(b_obj, point.contact_force) + contact_points[-1].force_x_in_world_frame = point.contact_force[0] + contact_points[-1].force_y_in_world_frame = point.contact_force[1] + contact_points[-1].force_z_in_world_frame = point.contact_force[2] + contact_points[-1].normal_on_b = point.contact_force[2] + contact_points[-1].normal_force = point.contact_force[2] return contact_points @staticmethod @@ -323,6 +327,8 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L """ obj_quat = obj.get_orientation_as_list() obj_rot_matrix = quaternion_matrix(obj_quat)[:3, :3] + # invert the rotation matrix to get the transformation from world to object frame + obj_rot_matrix = np.linalg.inv(obj_rot_matrix) contact_force_array = obj_rot_matrix @ np.array(contact_force).reshape(3, 1) return contact_force_array.flatten().tolist()[2] diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ca6aa682a..bcc2f5f08 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -111,7 +111,7 @@ def test_set_joint_position(self): break i += 1 # time.sleep(0.1) - self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.001) + self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.03) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -144,7 +144,7 @@ def test_set_robot_position(self): new_position = [1, 1, 0.1] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() - self.assert_positon_is_equal(robot_position, new_position, delta=0.035) + self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.02) def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) @@ -194,31 +194,30 @@ def test_attach_with_robot(self): self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): - milk = self.spawn_milk([1, 0, 0.1]) - cup = self.spawn_cup([1, 1, 0.1]) + milk = self.spawn_milk([1, 0, 0.1], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) - cup_position = cup.get_position_as_list() - milk.set_position([cup_position[0], cup_position[1], cup_position[2] + 0.2]) - contact_points = self.multiverse.get_object_contact_points(milk) + cup = self.spawn_cup([1, 0, 0.2]) + # rotate milk around y-axis by 90 degrees + contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) - self.assertTrue(contact_points[0].link_b.object, cup) + self.assertTrue(contact_points[0].link_b.object, milk) @staticmethod def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", pose=Pose([2, 2, 0.1], [0, 0, 0, 1])) - # time.sleep(0.1) return big_bowl @staticmethod - def spawn_milk(position: List) -> Object: + def spawn_milk(position: List, orientation: Optional[List] = None) -> Object: + if orientation is None: + orientation = [0, 0, 0, 1] milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", - pose=Pose(position, [0, 0, 0, 1])) - # time.sleep(0.5) + pose=Pose(position, orientation)) return milk def spawn_mobile_robot(self, position: Optional[List[float]] = None, robot_name: Optional[str] = 'tiago_dual') -> Object: @@ -237,14 +236,12 @@ def spawn_robot(self, position: Optional[List[float]] = None, else: robot = self.multiverse.robot robot.set_position(position) - # time.sleep(0.5) return robot @staticmethod def spawn_cup(position: List) -> Object: cup = Object("cup", ObjectType.GENERIC_OBJECT, "Cup.obj", pose=Pose(position, [0, 0, 0, 1])) - # time.sleep(0.5) return cup def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, From acbafdf3f9a6746474ce1cc8e5fd974e75fbab78 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 12 Jul 2024 17:24:26 +0200 Subject: [PATCH 047/189] [Multiverse] Added ray_test and ray_test_batch, all tests are running. --- .../world_concepts/multiverse_clients.py | 66 +++++++++++++++++++ src/pycram/worlds/multiverse.py | 29 +++++--- test/test_multiverse.py | 23 +++++++ 3 files changed, 110 insertions(+), 8 deletions(-) diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index 30a1cdfe6..b2ff58ebd 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -11,6 +11,23 @@ from ..datastructures.pose import Pose +@dataclass +class RayResult: + """ + A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the + distance from the ray origin to the intersection point. + """ + body_name: str + distance: float + + def intersected(self) -> bool: + """ + Check if the ray intersects with a body. + return: Whether the ray intersects with a body. + """ + return self.distance >= 0 and self.body_name != "" + + @dataclass class APIData: """ @@ -432,6 +449,55 @@ def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) for i in range(len(body_names))] + def get_objects_intersected_with_rays(self, from_positions: List[List[float]], + to_positions: List[List[float]]) -> List[RayResult]: + """ + Get the rays from the from_positions to the to_positions. + param from_positions: The from positions of the rays. + param to_positions: The to positions of the rays. + return: The rays as a list of RayResult. + """ + api_data = self._get_rays_api_data(from_positions, to_positions) + get_rays_response = self._request_single_api_callback(api_data) + return self._parse_get_rays_response(get_rays_response) + + def _get_rays_api_data(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> APIData: + """ + Get the rays API data to be added to the api callback request metadata. + param from_positions: The from positions of the rays. + param to_positions: The to positions of the rays. + return: The rays API data as an APIData. + """ + return APIData(self.GET_RAYS_API_NAME, [self.list_of_positions_to_string(from_positions), + self.list_of_positions_to_string(to_positions)] + ) + + @staticmethod + def _parse_get_rays_response(response: List[str]) -> List[RayResult]: + """ + Parse the response of the get rays API. + param response: The response of the get rays API as a list of strings. + return: The rays as a list of lists of floats. + """ + get_rays_results = [] + for ray_response in response: + if ray_response == "None": + get_rays_results.append(RayResult("", -1)) + else: + result = ray_response.split() + result[1] = float(result[1]) + get_rays_results.append(RayResult(*result)) + return get_rays_results + + @staticmethod + def list_of_positions_to_string(positions: List[List[float]]) -> str: + """ + Convert the list of positions to a string. + param positions: The list of positions. + return: The list of positions as a string. + """ + return " ".join([f"{position[0]} {position[1]} {position[2]}" for position in positions]) + @staticmethod def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 726e42858..96c91f2aa 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -290,6 +290,9 @@ def perform_collision_detection(self) -> None: logging.warning("perform_collision_detection is not implemented in Multiverse") def get_object_contact_points(self, obj: Object) -> ContactPointsList: + """ + Note: Currently Multiverse only gets one contact point per contact objects. + """ self.check_object_exists_and_issue_warning_if_not(obj) multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) @@ -310,7 +313,7 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: logging.error(f"Body link not found: {point.body_name}") raise ValueError contact_points.append(ContactPoint(obj.root_link, body_link)) - b_obj = body_link.object + # b_obj = body_link.object # normal_force_in_b_frame = self._get_normal_force_on_object_from_contact_force(b_obj, point.contact_force) contact_points[-1].force_x_in_world_frame = point.contact_force[0] contact_points[-1].force_y_in_world_frame = point.contact_force[1] @@ -335,17 +338,27 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: self.check_object_exists_and_issue_warning_if_not(obj1) self.check_object_exists_and_issue_warning_if_not(obj2) - logging.warning("get_contact_points_between_two_objects is not implemented in Multiverse") - return ContactPointsList([]) + obj1_contact_points = self.get_object_contact_points(obj1) + return obj1_contact_points.get_points_of_object(obj2) - def ray_test(self, from_position: List[float], to_position: List[float]) -> int: - logging.error("ray_test is not implemented in Multiverse") - raise NotImplementedError + def ray_test(self, from_position: List[float], to_position: List[float]) -> Optional[int]: + ray_test_result = self.ray_test_batch([from_position], [to_position]) + return ray_test_result[0] if ray_test_result[0] != -1 else None def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], num_threads: int = 1) -> List[int]: - logging.error("ray_test_batch is not implemented in Multiverse") - raise NotImplementedError + """ + Note: Currently, num_threads is not used in Multiverse. + """ + ray_results = self.api_requester.get_objects_intersected_with_rays(from_positions, to_positions) + results = [] + for ray_result in ray_results: + if ray_result.intersected(): + results.append(self.floor.id if ray_result.body_name == "world" else + self.object_name_to_id[ray_result.body_name]) + else: + results.append(-1) + return results def step(self): logging.warning("step is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index bcc2f5f08..269ceba02 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -206,6 +206,29 @@ def test_get_object_contact_points(self): self.assertEqual(len(contact_points), 1) self.assertTrue(contact_points[0].link_b.object, milk) + def test_get_contact_points_between_two_objects(self): + milk = self.spawn_milk([1, 0, 0.1], [0, -0.707, 0, 0.707]) + cup = self.spawn_cup([1, 0, 0.2]) + contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) + self.assertIsInstance(contact_points, list) + self.assertEqual(len(contact_points), 1) + self.assertTrue(contact_points[0].link_a.object, milk) + self.assertTrue(contact_points[0].link_b.object, cup) + + def test_get_one_ray(self): + milk = self.spawn_milk([1, 0, 0.1]) + intersected_object = self.multiverse.ray_test([1, 1, 0.1], [1, 0.5, 0.1]) + self.assertTrue(intersected_object is None) + intersected_object = self.multiverse.ray_test([1, 1, 0.1], [1, 0, 0.1]) + self.assertTrue(intersected_object == milk.id) + + def test_get_rays(self): + milk = self.spawn_milk([1, 0, 0.1]) + intersected_objects = self.multiverse.ray_test_batch([[1, 1, 0.1], [1, 1, 0.1]], + [[1, 0.5, 0.1], [1, 0, 0.1]]) + self.assertTrue(intersected_objects[0] == -1) + self.assertTrue(intersected_objects[1] == milk.id) + @staticmethod def spawn_big_bowl() -> Object: big_bowl = Object("big_bowl", ObjectType.GENERIC_OBJECT, "BigBowl.obj", From d62dd4d30bf6c0742294caab8b0f68387b60ba23 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 12 Jul 2024 19:48:25 +0200 Subject: [PATCH 048/189] [Multiverse] Testing multiverse demo, corrected cache file name. --- .../pycram_multiverse_demo/multiverse_demo.py | 90 +++++++++++++++++++ src/pycram/cache_manager.py | 2 +- src/pycram/datastructures/world.py | 3 +- src/pycram/world_concepts/world_object.py | 3 +- src/pycram/worlds/multiverse.py | 25 +++--- test/test_multiverse.py | 23 +++++ 6 files changed, 132 insertions(+), 14 deletions(-) create mode 100644 demos/pycram_multiverse_demo/multiverse_demo.py diff --git a/demos/pycram_multiverse_demo/multiverse_demo.py b/demos/pycram_multiverse_demo/multiverse_demo.py new file mode 100644 index 000000000..ab993e0d2 --- /dev/null +++ b/demos/pycram_multiverse_demo/multiverse_demo.py @@ -0,0 +1,90 @@ +from pycram.worlds.multiverse import Multiverse +from pycram.designators.action_designator import * +from pycram.designators.location_designator import * +from pycram.designators.object_designator import * +from pycram.datastructures.enums import ObjectType +from pycram.datastructures.pose import Pose +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.world_concepts.world_object import Object +from pycram.datastructures.dataclasses import Color + +extension = ObjectDescription.get_file_extension() + +world = Multiverse() +robot = Object("tiago_dual", ObjectType.ROBOT, f"tiago_dual{extension}", pose=Pose([1, 2, 0])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + +milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), + color=Color(1, 0, 0, 1)) +spoon = Object("soup_spoon", ObjectType.SPOON, f"soup_spoon{extension}", pose=Pose([2.4, 2.2, 0.85]), + color=Color(0, 0, 1, 1)) +bowl = Object("big_bowl", ObjectType.BOWL, f"big_bowl{extension}", pose=Pose([2.5, 2.2, 1.02]), + color=Color(1, 1, 0, 1)) +apartment.attach(spoon, 'cabinet10_drawer_top') + +pick_pose = Pose([2.7, 2.15, 1]) + +robot_desig = BelieveObject(names=["tiago_dual"]) +apartment_desig = BelieveObject(names=["apartment"]) + + +@with_simulated_robot +def move_and_detect(obj_type): + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[pick_pose]).resolve().perform() + + object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + + return object_desig + + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + milk_desig = move_and_detect(ObjectType.MILK) + + TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + + bowl_desig = move_and_detect(ObjectType.BOWL) + + TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + + # Finding and navigating to the drawer holding the spoon + handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) + drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), + robot_desig=robot_desig.resolve()).resolve() + + NavigateAction([drawer_open_loc.pose]).resolve().perform() + + OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + spoon.detach(apartment) + + # Detect and pickup the spoon + LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform() + + spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() + + pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right" + PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform() + + ParkArmsAction([Arms.LEFT if pickup_arm == "left" else Arms.RIGHT]).resolve().perform() + + CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.15]).resolve().perform() + + # Find a pose to place the spoon, move and then place it + spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1]) + placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() + + NavigateAction([placing_loc.pose]).resolve().perform() + + PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 7737be58d..3cc0c254c 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -125,7 +125,7 @@ def check_with_extension(self, path: str) -> bool: :param path: The path of the file to check. """ file_name = pathlib.Path(path).name - full_path = pathlib.Path(self.cache_dir + file_name) + full_path = pathlib.Path(os.path.join(self.cache_dir, file_name)) return full_path.exists() def check_without_extension(self, path: str, object_description: 'ObjectDescription') -> bool: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index c6fbb325d..7065f666a 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -864,8 +864,7 @@ def reset_world_and_remove_objects(self) -> None: """ self.reset_world() for obj in copy(self.objects): - if obj.name != 'floor': - self.remove_object(obj) + self.remove_object(obj) def reset_world(self, remove_saved_states=True) -> None: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 75c3e5641..af43d72f5 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -74,6 +74,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose + self.world.objects.append(self) + self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) self.description.update_description_from_file(self.path) @@ -98,7 +100,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if self.name == "spoon" and self.world.is_prospection_world: print("spoon problem") - self.world.objects.append(self) @property def pose(self): diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 96c91f2aa..43c8b779a 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -95,9 +95,10 @@ class Multiverse(World): A flag to check if the multiverse resources have been added. """ - def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT, + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, - simulation_frequency: Optional[float] = 60.0): + simulation_frequency: Optional[float] = 60.0, + simulation: Optional[str] = "empty_simulation"): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -105,12 +106,13 @@ def __init__(self, simulation: str, mode: Optional[WorldMode] = WorldMode.DIRECT param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. """ + + self._make_sure_multiverse_resources_are_added() + World.__init__(self, mode, is_prospection, simulation_frequency) self._init_clients(simulation) - self._make_sure_multiverse_resources_are_added() - self._set_world_job_flags() self._init_constraint_and_object_id_name_map_collections() @@ -144,8 +146,7 @@ def _make_sure_multiverse_resources_are_added(self): if not self.added_multiverse_resources: dirname = find_multiverse_resources_path() resources_paths = get_resource_paths(dirname) - for resource_path in resources_paths: - self.add_resource_path(resource_path) + World.data_directory = resources_paths + self.data_directory self.added_multiverse_resources = True def _spawn_floor(self): @@ -172,7 +173,8 @@ def load_object_and_get_id(self, name: Optional[str] = None, if pose is None: pose = Pose() - self._set_body_pose(name, pose) + if not self.get_object_by_name(name).obj_type == ObjectType.ENVIRONMENT: + self._set_body_pose(name, pose) return self._update_object_id_name_maps_and_get_latest_id(name) @@ -216,6 +218,8 @@ def get_link_pose(self, link: Link) -> Pose: def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) + if obj.obj_type == ObjectType.ENVIRONMENT: + return Pose() return self._get_body_pose(obj.name) def _get_body_pose(self, body_name: str) -> Pose: @@ -224,12 +228,12 @@ def _get_body_pose(self, body_name: str) -> Pose: param body_name: The name of the body. return: The pose of the body. """ - if body_name == "floor": - return Pose() return self.reader.get_body_pose(body_name) def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) + if obj.obj_type == ObjectType.ENVIRONMENT: + return self._set_body_pose(obj.name, pose) def multiverse_reset_world(self): @@ -256,7 +260,8 @@ def join_threads(self) -> None: self.reader.join() def remove_object_from_simulator(self, obj: Object) -> None: - self.writer.remove_body(obj.name) + if obj.obj_type != ObjectType.ENVIRONMENT: + self.writer.remove_body(obj.name) def add_constraint(self, constraint: Constraint) -> int: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 269ceba02..876292581 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -5,8 +5,11 @@ import psutil from typing_extensions import Optional, List +from pycram.datastructures.dataclasses import Color from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose +from pycram.designators.object_designator import BelieveObject +from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object multiverse_installed = True @@ -51,6 +54,26 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() # MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() + def test_demo(self): + extension = ObjectDescription.get_file_extension() + + robot = Object("tiago_dual", ObjectType.ROBOT, f"tiago_dual{extension}", pose=Pose([1, 2, 0.01])) + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + + milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), + color=Color(1, 0, 0, 1)) + spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.5, 2.5, 1.02]), + color=Color(0, 0, 1, 1)) + bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.2, 1.02]), + color=Color(1, 1, 0, 1)) + bowl.attach(spoon) + bowl.set_position([2.5, 2.3, 1.02]) + + pick_pose = Pose([2.7, 2.15, 1]) + + robot_desig = BelieveObject(names=["tiago_dual"]) + apartment_desig = BelieveObject(names=["apartment"]) + def test_reset_world(self): set_position = [1, 1, 0.1] milk = self.spawn_milk(set_position) From b73f02e3a26ef3838eeee89c4f74bf882bae10e1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 12 Jul 2024 22:23:19 +0200 Subject: [PATCH 049/189] [Multiverse] Set multiple joints at same time, tests are running. --- src/pycram/datastructures/world.py | 9 +++ .../world_concepts/multiverse_clients.py | 32 ++++++++-- src/pycram/world_concepts/world_object.py | 5 +- src/pycram/worlds/bullet_world.py | 6 +- src/pycram/worlds/multiverse.py | 14 ++++- test/test_multiverse.py | 58 +++++++++---------- 6 files changed, 81 insertions(+), 43 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 7065f666a..75304ef63 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1094,6 +1094,15 @@ def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: def __del__(self): self.exit() + @abstractmethod + def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: + """ + Set the positions of multiple joints of an articulated object. + :param obj: The object. + :param joint_poses: A dictionary with joint names as keys and joint positions as values. + """ + pass + class UseProspectionWorld: """ diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index b2ff58ebd..9d79cff02 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -281,12 +281,34 @@ def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) param data: The data to be sent. return: The response from the server. """ - send_meta_data = {body_name: list(data.keys())} - flattened_data = [item for sublist in data.values() for item in sublist] - send_data = [time() - self.time_start, *flattened_data] - response = self.send_data_to_server(send_data, send_meta_data=send_meta_data) + return self.send_multiple_body_data_to_server({body_name: data}) + + def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[float]]]) -> Dict: + """ + Send data to the multiverse server for multiple bodies. + param body_data: The data to be sent for multiple bodies. + return: The response from the server. + """ + send_meta_data = {body_name: list(data.keys()) for body_name, data in body_data.items()} + response_meta_data = self.send_meta_data_and_get_response(send_meta_data) + body_names = list(response_meta_data["send"].keys()) + flattened_data = [value for body_name in body_names for data in body_data[body_name].values() + for value in data] + self.send_data = [time() - self.time_start, *flattened_data] + self.send_and_receive_data() sleep(self.time_for_sim_update) - return response + return self.response_meta_data + + def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: + """ + Send metadata to the multiverse server and get the response. + param send_meta_data: The metadata to be sent. + return: The response from the server. + """ + self._reset_request_meta_data() + self.request_meta_data["send"] = send_meta_data + self.send_and_receive_meta_data() + return self.response_meta_data def send_data_to_server(self, data: List, send_meta_data: Optional[Dict] = None, diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index af43d72f5..15a393a84 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -824,6 +824,8 @@ def reset_all_joints_positions(self) -> None: Sets the current position of all joints to 0. This is useful if the joints should be reset to their default """ joint_names = list(self.joint_name_to_id.keys()) + if len(joint_names) == 0: + return joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) @@ -834,8 +836,7 @@ def set_joint_positions(self, joint_poses: dict) -> None: :param joint_poses: """ - for joint_name, joint_position in joint_poses.items(): - self.joints[joint_name].position = joint_position + self.world.set_multiple_joint_positions(self, joint_poses) # self.update_pose() self._update_all_links_poses() self.update_link_transforms() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 21c99f8ad..b62691d4b 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -163,7 +163,11 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: str) -> None: + def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: + for joint_name, joint_position in joint_poses.items(): + self.reset_joint_position(obj.joints[joint_name], joint_position) + + def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 43c8b779a..85b532507 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -207,11 +207,19 @@ def get_body_property(self, body_name: str, property_name: str) -> float: def reset_joint_position(self, joint: Joint, joint_position: float) -> None: attribute = self.get_joint_position_name(joint) - self.send_data_to_server(joint.name, {attribute: [joint_position]}) + self.send_body_data_to_server(joint.name, {attribute: [joint_position]}) - def send_data_to_server(self, body_name: str, data: Dict[str, List[float]]): + def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: + data = {joint.name: {self.get_joint_position_name(joint): [joint_poses[joint.name]]} + for joint in obj.joints.values()} + self.writer.send_multiple_body_data_to_server(data) + + def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]): self.writer.send_body_data_to_server(body_name, data) + def sent_data_to_server(self, data: Dict[str, List[float]]): + self.writer.send_data_to_server(list(data.values())) + def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) return self._get_body_pose(link.name) @@ -304,7 +312,7 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: body_link = None for point in multiverse_contact_points: if point.body_name == "world": - continue + point.body_name = "floor" body_object = self.get_object_by_name(point.body_name) if body_object is None: for obj in self.objects: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 876292581..5829aa3e3 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -88,24 +88,24 @@ def test_reset_world(self): milk.original_pose.orientation_as_list()) def test_spawn_object(self): - milk = self.spawn_milk([1, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() - self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 0]) + self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 1]) self.assert_list_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_remove_object(self): - milk = self.spawn_milk([0, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) milk.remove() self.assertTrue(milk not in self.multiverse.objects) self.assertFalse(self.multiverse.check_object_exists_in_multiverse(milk.name)) def test_check_object_exists(self): - milk = self.spawn_milk([0, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) self.assertTrue(self.multiverse.check_object_exists_in_multiverse(milk.name)) def test_set_position(self): - milk = self.spawn_milk([0, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) original_milk_position = milk.get_position_as_list() original_milk_position[0] += 1 milk.set_position(original_milk_position) @@ -113,10 +113,10 @@ def test_set_position(self): self.assert_list_is_equal(milk_position[:2], original_milk_position[:2]) def test_update_position(self): - milk = self.spawn_milk([1, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], [1, 0]) + self.assert_list_is_equal(milk_position[:2], [1, 1]) def test_set_joint_position(self): if self.multiverse.robot is None: @@ -125,16 +125,10 @@ def test_set_joint_position(self): robot = self.multiverse.robot original_joint_position = robot.get_joint_position("joint1") step = 1.57 - i = 0 - while True: - robot.set_joint_position("joint1", original_joint_position - step * i) - robot.joints["joint1"]._update_position() - joint_position = robot.get_joint_position("joint1") - if joint_position <= original_joint_position - 1.57: - break - i += 1 - # time.sleep(0.1) - self.assertAlmostEqual(joint_position, original_joint_position - 1.57, delta=0.03) + robot.set_joint_position("joint1", original_joint_position - step) + robot.joints["joint1"]._update_position() + joint_position = robot.get_joint_position("joint1") + self.assertAlmostEqual(joint_position, original_joint_position - step, delta=0.3) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -158,16 +152,16 @@ def test_respawn_robot(self): self.assertTrue(self.multiverse.robot in self.multiverse.objects) self.multiverse.robot.remove() self.assertTrue(self.multiverse.robot not in self.multiverse.objects) - self.spawn_robot(position=[0, 0, 0.1]) + self.spawn_robot() self.assertTrue(self.multiverse.robot in self.multiverse.objects) # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): self.spawn_mobile_robot(robot_name='panda_free') - new_position = [1, 1, 0.1] + new_position = [-3, -3, 0.1] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() - self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.02) + self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) @@ -204,7 +198,7 @@ def test_detach_object(self): self.tearDown() def test_attach_with_robot(self): - milk = self.spawn_milk([1, 0, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) robot = self.spawn_robot() # Get position of milk relative to robot end effector milk_initial_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) @@ -217,12 +211,12 @@ def test_attach_with_robot(self): self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): - milk = self.spawn_milk([1, 0, 0.1], [0, -0.707, 0, 0.707]) + milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) - cup = self.spawn_cup([1, 0, 0.2]) + cup = self.spawn_cup([1, 1, 0.2]) # rotate milk around y-axis by 90 degrees contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, list) @@ -230,8 +224,8 @@ def test_get_object_contact_points(self): self.assertTrue(contact_points[0].link_b.object, milk) def test_get_contact_points_between_two_objects(self): - milk = self.spawn_milk([1, 0, 0.1], [0, -0.707, 0, 0.707]) - cup = self.spawn_cup([1, 0, 0.2]) + milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) + cup = self.spawn_cup([1, 1, 0.2]) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, list) self.assertEqual(len(contact_points), 1) @@ -239,16 +233,16 @@ def test_get_contact_points_between_two_objects(self): self.assertTrue(contact_points[0].link_b.object, cup) def test_get_one_ray(self): - milk = self.spawn_milk([1, 0, 0.1]) - intersected_object = self.multiverse.ray_test([1, 1, 0.1], [1, 0.5, 0.1]) + milk = self.spawn_milk([1, 1, 0.1]) + intersected_object = self.multiverse.ray_test([1, 2, 0.1], [1, 1.5, 0.1]) self.assertTrue(intersected_object is None) - intersected_object = self.multiverse.ray_test([1, 1, 0.1], [1, 0, 0.1]) + intersected_object = self.multiverse.ray_test([1, 2, 0.1], [1, 1, 0.1]) self.assertTrue(intersected_object == milk.id) def test_get_rays(self): - milk = self.spawn_milk([1, 0, 0.1]) - intersected_objects = self.multiverse.ray_test_batch([[1, 1, 0.1], [1, 1, 0.1]], - [[1, 0.5, 0.1], [1, 0, 0.1]]) + milk = self.spawn_milk([1, 1, 0.1]) + intersected_objects = self.multiverse.ray_test_batch([[1, 2, 0.1], [1, 2, 0.1]], + [[1, 1.5, 0.1], [1, 1, 0.1]]) self.assertTrue(intersected_objects[0] == -1) self.assertTrue(intersected_objects[1] == milk.id) @@ -273,7 +267,7 @@ def spawn_robot(self, position: Optional[List[float]] = None, robot_name: Optional[str] = 'panda', replace: Optional[bool] = False) -> Object: if position is None: - position = [0, 0, 0.1] + position = [-2, -2, 0.1] if self.multiverse.robot is None or replace: if self.multiverse.robot is not None: self.multiverse.robot.remove() From 324ef2fea6a0b3a5678a5e59498ed1ead53b124f Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 15 Jul 2024 16:25:53 +0200 Subject: [PATCH 050/189] [Multiverse] robot set pose in progress --- src/pycram/datastructures/dataclasses.py | 8 + src/pycram/datastructures/world.py | 6 +- .../world_concepts/multiverse_clients.py | 3 +- src/pycram/world_concepts/world_object.py | 2 +- src/pycram/worlds/multiverse.py | 150 ++++++++++-------- test/test_multiverse.py | 73 +++++---- 6 files changed, 137 insertions(+), 105 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index ff28053a9..bac146528 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -331,6 +331,14 @@ def __repr__(self): class ContactPointsList(list): + def __index__(self, index: int) -> ContactPoint: + return super().__getitem__(index) + + def __getitem__(self, item) -> Union[ContactPoint, 'ContactPointsList']: + if isinstance(item, slice): + return ContactPointsList(super().__getitem__(item)) + return super().__getitem__(item) + def get_normals_of_object(self, obj: Object) -> List[List[float]]: """ Gets the normals of the object. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 75304ef63..dd6094b0b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -335,7 +335,8 @@ def remove_object(self, obj: Object) -> None: self.object_lock.acquire() obj.detach_all() - self.objects.remove(obj) + if obj.name != "floor": + self.objects.remove(obj) # This means the current world of the object is not the prospection world, since it # has a reference to the prospection world @@ -343,7 +344,8 @@ def remove_object(self, obj: Object) -> None: self.world_sync.remove_obj_queue.put(obj) self.world_sync.remove_obj_queue.join() - self.remove_object_from_simulator(obj) + if obj.name != "floor": + self.remove_object_from_simulator(obj) if World.robot == obj: World.robot = None diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index 9d79cff02..b044f335d 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -202,7 +202,7 @@ def join(self): class MultiverseWriter(MultiverseSocket): - time_for_sim_update: Optional[float] = 0.4 + time_for_sim_update: Optional[float] = 0.7 """ Wait time for the sent data to be applied in the simulator. """ @@ -267,6 +267,7 @@ def remove_body(self, body_name: str) -> None: self.send_data_to_server([time() - self.time_start], send_meta_data={body_name: []}, receive_meta_data={body_name: []}) + sleep(self.time_for_sim_update) def reset_world(self) -> None: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 15a393a84..9348be6ad 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -67,7 +67,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color - self.description = description() + self.description: ObjectDescription = description() self.cache_manager = self.world.cache_manager self.local_transformer = LocalTransformer() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 85b532507..ae5dab407 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -15,68 +15,6 @@ from ..world_concepts.world_object import Object -def get_resource_paths(dirname: str) -> List[str]: - resources_paths = ["../robots", "../worlds", "../objects"] - resources_paths = [ - os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( - resources_path) else resources_path - for resources_path in resources_paths - ] - - def add_directories(path: str) -> None: - with os.scandir(path) as entries: - for entry in entries: - if entry.is_dir(): - resources_paths.append(entry.path) - add_directories(entry.path) - - resources_path_copy = resources_paths.copy() - for resources_path in resources_path_copy: - add_directories(resources_path) - - return resources_paths - - -def find_multiverse_resources_path() -> Optional[str]: - """ - Find the path to the Multiverse resources directory. - """ - # Get the path to the Multiverse installation - multiverse_path = find_multiverse_path() - - # Check if the path to the Multiverse installation was found - if multiverse_path: - # Construct the path to the resources directory - resources_path = os.path.join(multiverse_path, 'resources') - - # Check if the resources directory exists - if os.path.exists(resources_path): - return resources_path - - return None - - -def find_multiverse_path() -> Optional[str]: - """ - Find the path to the Multiverse installation. - """ - # Get the value of PYTHONPATH environment variable - pythonpath = os.getenv('PYTHONPATH') - - # Check if PYTHONPATH is set - if pythonpath: - # Split the PYTHONPATH into individual paths using the platform-specific path separator - paths = pythonpath.split(os.pathsep) - - # Iterate through each path and check if 'Multiverse' is in it - for path in paths: - if 'multiverse' in path: - multiverse_path = path.split('multiverse')[0] - return multiverse_path + 'multiverse' - - return None - - class Multiverse(World): """ This class implements an interface between Multiverse and PyCRAM. @@ -190,9 +128,8 @@ def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: return self.last_object_id def get_object_joint_names(self, obj: Object) -> List[str]: - return [joint.name for joint in obj.description.joints - if joint.type in [JointType.REVOLUTE, JointType.PRISMATIC]] + if joint.type in self._joint_type_to_position_name.keys()] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] @@ -242,7 +179,26 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return - self._set_body_pose(obj.name, pose) + # elif obj.obj_type == ObjectType.ROBOT: + # self.set_mobile_robot_pose(obj, pose) + else: + self._set_body_pose(obj.name, pose) + + def set_mobile_robot_pose(self, robot: Object, pose: Pose): + # Get the joints of the base link + base_link = robot.root_link + base_link_joints = [joint for joint in robot.joints.values() if joint.child_link == base_link] + joint_name_position_dict = {} + for joint in base_link_joints: + # get the pose in the joint frame + pose_in_joint_frame = robot.local_transformer.transform_pose(pose, joint.tf_frame) + if joint.type == JointType.PRISMATIC: + joint_name_position_dict[joint.name] = pose_in_joint_frame.position[joint.axis.index(1)] + elif joint.axis == [0, 0, 1] and joint.type == JointType.REVOLUTE: + # get rotation angle around the z-axis + angle = pose_in_joint_frame.orientation.to_euler().z + joint_name_position_dict[joint.name] = angle + self.set_multiple_joint_positions(robot, joint_name_position_dict) def multiverse_reset_world(self): self.writer.reset_world() @@ -324,7 +280,7 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: body_link = body_object.root_link if body_link is None: logging.error(f"Body link not found: {point.body_name}") - raise ValueError + raise ValueError(f"Body link not found: {point.body_name}") contact_points.append(ContactPoint(obj.root_link, body_link)) # b_obj = body_link.object # normal_force_in_b_frame = self._get_normal_force_on_object_from_contact_force(b_obj, point.contact_force) @@ -423,3 +379,65 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): def check_object_exists_in_multiverse(self, object_name: str) -> bool: return self.api_requester.check_object_exists(object_name) + + +def get_resource_paths(dirname: str) -> List[str]: + resources_paths = ["../robots", "../worlds", "../objects"] + resources_paths = [ + os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( + resources_path) else resources_path + for resources_path in resources_paths + ] + + def add_directories(path: str) -> None: + with os.scandir(path) as entries: + for entry in entries: + if entry.is_dir(): + resources_paths.append(entry.path) + add_directories(entry.path) + + resources_path_copy = resources_paths.copy() + for resources_path in resources_path_copy: + add_directories(resources_path) + + return resources_paths + + +def find_multiverse_resources_path() -> Optional[str]: + """ + Find the path to the Multiverse resources directory. + """ + # Get the path to the Multiverse installation + multiverse_path = find_multiverse_path() + + # Check if the path to the Multiverse installation was found + if multiverse_path: + # Construct the path to the resources directory + resources_path = os.path.join(multiverse_path, 'resources') + + # Check if the resources directory exists + if os.path.exists(resources_path): + return resources_path + + return None + + +def find_multiverse_path() -> Optional[str]: + """ + Find the path to the Multiverse installation. + """ + # Get the value of PYTHONPATH environment variable + pythonpath = os.getenv('PYTHONPATH') + + # Check if PYTHONPATH is set + if pythonpath: + # Split the PYTHONPATH into individual paths using the platform-specific path separator + paths = pythonpath.split(os.pathsep) + + # Iterate through each path and check if 'Multiverse' is in it + for path in paths: + if 'multiverse' in path: + multiverse_path = path.split('multiverse')[0] + return multiverse_path + 'multiverse' + + return None \ No newline at end of file diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5829aa3e3..348643546 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -5,7 +5,7 @@ import psutil from typing_extensions import Optional, List -from pycram.datastructures.dataclasses import Color +from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose from pycram.designators.object_designator import BelieveObject @@ -57,7 +57,7 @@ def tearDown(self): def test_demo(self): extension = ObjectDescription.get_file_extension() - robot = Object("tiago_dual", ObjectType.ROBOT, f"tiago_dual{extension}", pose=Pose([1, 2, 0.01])) + robot = self.spawn_robot(robot_name='tiago_dual', position=[1, 2, 0.01]) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), @@ -120,15 +120,15 @@ def test_update_position(self): def test_set_joint_position(self): if self.multiverse.robot is None: - robot = self.spawn_robot() + robot = self.spawn_robot('tiago_dual') else: robot = self.multiverse.robot - original_joint_position = robot.get_joint_position("joint1") - step = 1.57 - robot.set_joint_position("joint1", original_joint_position - step) - robot.joints["joint1"]._update_position() - joint_position = robot.get_joint_position("joint1") - self.assertAlmostEqual(joint_position, original_joint_position - step, delta=0.3) + step = 0.2 + for joint in ['torso_lift_joint', 'head_1_joint']: + original_joint_position = robot.get_joint_position(joint) + robot.set_joint_position(joint, original_joint_position - step) + joint_position = robot.get_joint_position(joint) + self.assertAlmostEqual(joint_position, original_joint_position - step, delta=0.01) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -157,8 +157,8 @@ def test_respawn_robot(self): # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): - self.spawn_mobile_robot(robot_name='panda_free') - new_position = [-3, -3, 0.1] + self.spawn_robot(robot_name='tiago_dual') + new_position = [-3, -3, 0.001] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) @@ -193,7 +193,7 @@ def test_detach_object(self): milk.set_position(milk_position) new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.002) + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.005) self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) self.tearDown() @@ -211,26 +211,32 @@ def test_attach_with_robot(self): self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): - milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) - contact_points = self.multiverse.get_object_contact_points(milk) - self.assertIsInstance(contact_points, list) - self.assertEqual(len(contact_points), 1) - self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) - cup = self.spawn_cup([1, 1, 0.2]) - # rotate milk around y-axis by 90 degrees - contact_points = self.multiverse.get_object_contact_points(cup) - self.assertIsInstance(contact_points, list) - self.assertEqual(len(contact_points), 1) - self.assertTrue(contact_points[0].link_b.object, milk) + for i in range(3): + milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) + contact_points = self.multiverse.get_object_contact_points(milk) + self.assertIsInstance(contact_points, ContactPointsList) + 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.1]) + contact_points = self.multiverse.get_object_contact_points(cup) + self.assertIsInstance(contact_points, ContactPointsList) + self.assertEqual(len(contact_points), 1) + self.assertIsInstance(contact_points[0], ContactPoint) + self.assertTrue(contact_points[0].link_b.object, milk) + self.tearDown() def test_get_contact_points_between_two_objects(self): - milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) - cup = self.spawn_cup([1, 1, 0.2]) - contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) - self.assertIsInstance(contact_points, list) - self.assertEqual(len(contact_points), 1) - self.assertTrue(contact_points[0].link_a.object, milk) - self.assertTrue(contact_points[0].link_b.object, cup) + for i in range(3): + milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) + cup = self.spawn_cup([1, 1, 0.2]) + contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) + self.assertIsInstance(contact_points, ContactPointsList) + self.assertEqual(len(contact_points), 1) + self.assertIsInstance(contact_points[0], ContactPoint) + self.assertTrue(contact_points[0].link_a.object, milk) + self.assertTrue(contact_points[0].link_b.object, cup) + self.tearDown() def test_get_one_ray(self): milk = self.spawn_milk([1, 1, 0.1]) @@ -260,14 +266,11 @@ def spawn_milk(position: List, orientation: Optional[List] = None) -> Object: pose=Pose(position, orientation)) return milk - def spawn_mobile_robot(self, position: Optional[List[float]] = None, robot_name: Optional[str] = 'tiago_dual') -> Object: - self.spawn_robot(position, robot_name, replace=True) - def spawn_robot(self, position: Optional[List[float]] = None, robot_name: Optional[str] = 'panda', - replace: Optional[bool] = False) -> Object: + replace: Optional[bool] = True) -> Object: if position is None: - position = [-2, -2, 0.1] + position = [-2, -2, 0.001] if self.multiverse.robot is None or replace: if self.multiverse.robot is not None: self.multiverse.robot.remove() From 7990b7ec43224fff28f9dd48562f3a625f945bb6 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 15 Jul 2024 21:01:55 +0200 Subject: [PATCH 051/189] [Multiverse] Prospection world is also working with normal spawn test. spawn robot causes a problem due to world_sync overloading I guess, need to check. Added VirtualMoveBaseJoints to move mobile_robots in the mujoco simulation. Added tiago_dual virtual move base joints to its description. --- src/pycram/datastructures/dataclasses.py | 7 ++ src/pycram/datastructures/world.py | 15 ++-- src/pycram/robot_description.py | 10 ++- .../robot_descriptions/tiago_description.py | 5 +- .../world_concepts/multiverse_clients.py | 28 +++++--- src/pycram/world_concepts/world_object.py | 13 ++-- src/pycram/worlds/bullet_world.py | 3 +- src/pycram/worlds/multiverse.py | 71 ++++++++++++------- test/test_multiverse.py | 2 +- 9 files changed, 104 insertions(+), 50 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index bac146528..e4d0e9350 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -420,3 +420,10 @@ class TextAnnotation: position: List[float] color: Color id: int + + +@dataclass +class VirtualMoveBaseJoints: + translation_x: str + translation_y: str + angular_z: str diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 1bc99541f..ff8bfa858 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -146,6 +146,11 @@ class World(StateEntity, ABC): Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ + prospection_world_prefix: str = "prospection_" + """ + The prefix for the prospection world name. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -170,15 +175,15 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_world(mode) + self.objects: List[Object] = [] + # List of all Objects in the World + self.is_prospection_world: bool = is_prospection_world self._init_and_sync_prospection_world() self.local_transformer = LocalTransformer() self._update_local_transformer_worlds() - self.objects: List[Object] = [] - # List of all Objects in the World - self.mode: WorldMode = mode # The mode of the simulation, can be "GUI" or "DIRECT" @@ -263,12 +268,14 @@ def simulation_time_step(self): return 1 / World.simulation_frequency @abstractmethod - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: """ Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. :param path: The path to the description file, if None the description file is assumed to be already loaded. :param pose: The pose at which the object should be loaded. + :param obj_type: The type of the object. :return: The id of the loaded object. """ pass diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index ce9805395..35274edda 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -5,6 +5,7 @@ from typing_extensions import List, Dict, Union, Optional from urdf_parser_py.urdf import URDF +from .datastructures.dataclasses import VirtualMoveBaseJoints from .utils import suppress_stdout_stderr from .datastructures.enums import Arms, Grasp, GripperState, GripperType @@ -105,8 +106,14 @@ class RobotDescription: """ All joints defined in the URDF, by default fixed joints are not included """ + virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None + """ + Virtual move base joint names for mobile robots, these joints are not part of the URDF, however they are used to + move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) + """ - def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str): + def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, + virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -129,6 +136,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.grasps: Dict[Grasp, List[float]] = {} self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] + self.virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = virtual_move_base_joints def add_kinematic_chain_description(self, chain: KinematicChainDescription): """ diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index 6a92d47ec..0c834694c 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -1,4 +1,6 @@ import rospkg + +from ..datastructures.dataclasses import VirtualMoveBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import GripperState, Arms, Grasp @@ -7,7 +9,8 @@ filename = rospack.get_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", - filename) + filename, VirtualMoveBaseJoints("odom_vel_lin_x_joint", "odom_vel_lin_y_joint", + "odom_vel_ang_z_joint")) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "torso_lift_link", "arm_left_7_link", diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index b044f335d..9fa8b75cb 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -8,6 +8,7 @@ from .constraints import Constraint from .multiverse_socket import MultiverseSocket, MultiverseMetaData, SocketAddress from .world_object import Object, Link +from ..datastructures.world import World from ..datastructures.pose import Pose @@ -63,16 +64,19 @@ class MultiverseContactPoint: class MultiverseReader(MultiverseSocket): - def __init__(self, max_wait_time_for_data: Optional[float] = 1): + def __init__(self, max_wait_time_for_data: Optional[float] = 1, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. This class provides methods to get data (e.g., position, orientation) from the Multiverse server. param max_wait_time_for_data: The maximum wait time for the data in seconds. + param is_prospection_world: Whether the reader is connected to the prospection world. """ self.max_wait_time_for_data = max_wait_time_for_data meta_data = MultiverseMetaData() - meta_data.simulation_name = "reader" - super().__init__(SocketAddress(port="8000"), meta_data) + meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "reader" + meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + port = "8003" if is_prospection_world else "8000" + super().__init__(SocketAddress(port=port), meta_data) self.run() self.data_lock = threading.Lock() self.thread = threading.Thread(target=self.receive_all_data_from_server) @@ -207,16 +211,19 @@ class MultiverseWriter(MultiverseSocket): Wait time for the sent data to be applied in the simulator. """ - def __init__(self, simulation: str): + def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. This class provides methods to send data (e.g., position, orientation) to the Multiverse server. param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). + param is_prospection_world: Whether the writer is connected to the prospection world. """ meta_data = MultiverseMetaData() - meta_data.simulation_name = "writer" - super().__init__(SocketAddress(port="8001"), meta_data) + meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "writer" + meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + port = "8004" if is_prospection_world else "8001" + super().__init__(SocketAddress(port=port), meta_data) self.simulation = simulation self.time_start = time() self.run() @@ -344,16 +351,19 @@ class MultiverseAPI(MultiverseSocket): The API names for the API callbacks to the Multiverse server. """ - def __init__(self, simulation: str): + def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. This class provides methods like attach and detach objects, get contact points, and other API requests. param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). + param is_prospection_world: Whether the API is connected to the prospection world. """ meta_data = MultiverseMetaData() - meta_data.simulation_name = "api_requester" - super().__init__(SocketAddress(port="8002"), meta_data) + meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "api_requester" + meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + port = "8005" if is_prospection_world else "8002" + super().__init__(SocketAddress(port=port), meta_data) self.simulation = simulation self.run() diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index c84a1228b..59912a269 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -10,7 +10,6 @@ from ..description import ObjectDescription, LinkDescription, Joint from ..object_descriptors.urdf import ObjectDescription as URDFObject -from ..robot_descriptions import robot_description from ..datastructures.world import WorldEntity, World from ..world_concepts.constraints import Attachment from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, @@ -31,8 +30,7 @@ class Object(WorldEntity): prospection_world_prefix: str = "prospection/" """ - The ObjectDescription of the object, this contains the name and type of the object as well as the path to the source - file. + The prefix for the tf frame of objects in the prospection world. """ def __init__(self, name: str, obj_type: ObjectType, path: str, @@ -64,7 +62,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, pose = Pose() if name in [obj.name for obj in self.world.objects]: rospy.logerr(f"An object with the name {name} already exists in the world.") - return None + raise ValueError(f"An object with the name {name} already exists in the world.") self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color @@ -79,8 +77,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.description.update_description_from_file(self.path) - self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") - + f"{self.name}") + self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name self._init_joint_name_and_id_map() self._init_link_name_and_id_map() @@ -93,13 +90,13 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if not self.world.is_prospection_world: self._add_to_world_sync_obj_queue() - self.world.objects.append(self) - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: rdm = RobotDescriptionManager() rdm.load_description(self.name) World.robot = self + self.world.objects.append(self) + @property def pose(self): return self.get_pose() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 114840c9b..7af5ca325 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -69,7 +69,8 @@ def _init_world(self, mode: WorldMode): self._gui_thread.start() time.sleep(0.1) - def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int: + def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: if pose is None: pose = Pose() return self._load_object_and_get_id(path, pose) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ae5dab407..07bb28389 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -2,14 +2,16 @@ import os import numpy as np -from tf.transformations import quaternion_matrix +from tf.transformations import quaternion_matrix, euler_from_quaternion from typing_extensions import List, Dict, Optional -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, \ + VirtualMoveBaseJoints from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint +from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.multiverse_clients import MultiverseReader, MultiverseWriter, MultiverseAPI from ..world_concepts.world_object import Object @@ -33,10 +35,16 @@ class Multiverse(World): A flag to check if the multiverse resources have been added. """ + simulation: Optional[str] = None + """ + The simulation name to be used in the Multiverse world (this is the name defined in + the multiverse configuration file). + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, - simulation: Optional[str] = "empty_simulation"): + simulation: Optional[str] = None): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -47,20 +55,29 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._make_sure_multiverse_resources_are_added() + if Multiverse.simulation is None: + if simulation is None: + logging.error("Simulation name not provided") + raise ValueError("Simulation name not provided") + Multiverse.simulation = simulation + + self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + World.__init__(self, mode, is_prospection, simulation_frequency) - self._init_clients(simulation) + self._init_clients() self._set_world_job_flags() self._init_constraint_and_object_id_name_map_collections() - self._spawn_floor() + if not self.is_prospection_world: + self._spawn_floor() - def _init_clients(self, simulation: str): - self.reader = MultiverseReader() - self.writer = MultiverseWriter(simulation) - self.api_requester = MultiverseAPI(simulation) + def _init_clients(self): + self.reader = MultiverseReader(is_prospection_world=self.is_prospection_world) + self.writer = MultiverseWriter(self.simulation, is_prospection_world=self.is_prospection_world) + self.api_requester = MultiverseAPI(self.simulation, is_prospection_world=self.is_prospection_world) def _set_world_job_flags(self): self.let_pycram_move_attached_objects = False @@ -101,17 +118,19 @@ def get_joint_position_name(self, joint: Joint) -> str: return self._joint_type_to_position_name[joint.type] def load_object_and_get_id(self, name: Optional[str] = None, - pose: Optional[Pose] = None) -> int: + pose: Optional[Pose] = None, + obj_type: Optional[ObjectType] = None) -> int: """ Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as the name of the object in the description file. param name: The name of the object to be loaded. param pose: The pose of the object. + param obj_type: The type of the object. """ if pose is None: pose = Pose() - if not self.get_object_by_name(name).obj_type == ObjectType.ENVIRONMENT: + if not obj_type == ObjectType.ENVIRONMENT: self._set_body_pose(name, pose) return self._update_object_id_name_maps_and_get_latest_id(name) @@ -179,25 +198,23 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return - # elif obj.obj_type == ObjectType.ROBOT: - # self.set_mobile_robot_pose(obj, pose) + elif obj.obj_type == ObjectType.ROBOT and RobotDescription.virtual_move_base_joints is not None: + self.set_mobile_robot_pose(obj, pose) else: self._set_body_pose(obj.name, pose) def set_mobile_robot_pose(self, robot: Object, pose: Pose): + """ + Set the pose of a mobile robot in the simulator by setting the position of the virtual move base joints. + param robot: The robot object. + param pose: The pose of the robot. + """ # Get the joints of the base link - base_link = robot.root_link - base_link_joints = [joint for joint in robot.joints.values() if joint.child_link == base_link] - joint_name_position_dict = {} - for joint in base_link_joints: - # get the pose in the joint frame - pose_in_joint_frame = robot.local_transformer.transform_pose(pose, joint.tf_frame) - if joint.type == JointType.PRISMATIC: - joint_name_position_dict[joint.name] = pose_in_joint_frame.position[joint.axis.index(1)] - elif joint.axis == [0, 0, 1] and joint.type == JointType.REVOLUTE: - # get rotation angle around the z-axis - angle = pose_in_joint_frame.orientation.to_euler().z - joint_name_position_dict[joint.name] = angle + base_link_joints: VirtualMoveBaseJoints = RobotDescription.virtual_move_base_joints + joint_name_position_dict = {base_link_joints.translation_x: pose.position.x, + base_link_joints.translation_y: pose.position.y, + base_link_joints.angular_z: euler_from_quaternion(pose.orientation_as_list())[2]} + # get the pose in the joint frame self.set_multiple_joint_positions(robot, joint_name_position_dict) def multiverse_reset_world(self): @@ -223,6 +240,10 @@ def join_threads(self) -> None: self.reader.stop_thread = True self.reader.join() + def remove_object_by_id(self, obj_id: int) -> None: + obj = self.get_object_by_id(obj_id) + self.remove_object_from_simulator(obj) + def remove_object_from_simulator(self, obj: Object) -> None: if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 348643546..5b872916e 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -42,7 +42,7 @@ def setUpClass(cls): if not multiverse_installed: return cls.multiverse = Multiverse(simulation="pycram_test", - is_prospection=True) + is_prospection=False) # cls.big_bowl = cls.spawn_big_bowl() @classmethod From 1dd7d1fecc2aeee11b93efb2a9796827ec5f074b Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 16 Jul 2024 09:54:34 +0200 Subject: [PATCH 052/189] [Multiverse] added a flag for sync handling in pycram vs simulator (e.g. multiverse). --- src/pycram/datastructures/world.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ff8bfa858..c315e5229 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -163,6 +163,11 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ StateEntity.__init__(self) + self.let_pycram_move_attached_objects = True + self.let_pycram_handle_spawning = True + self.let_pycram_handle_world_sync = True + self.update_poses_from_sim_on_get = False + if World.current_world is None: World.current_world = self World.simulation_frequency = simulation_frequency @@ -191,10 +196,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_events() - self.let_pycram_move_attached_objects = True - self.let_pycram_handle_spawning = True - self.update_poses_from_sim_on_get = False - self._current_state: Optional[WorldState] = None @abstractmethod From bd541a6d113b39ee6af359ae1151bac1db1324b5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 16 Jul 2024 16:31:59 +0200 Subject: [PATCH 053/189] [Multiverse] robot_descriptions are not being registered for some reason. --- src/pycram/datastructures/world.py | 118 +++++++++++++++++++++-------- 1 file changed, 88 insertions(+), 30 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index c315e5229..6bceee218 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -248,6 +248,7 @@ def _sync_prospection_world(self): self.world_sync = None else: self.world_sync: WorldSync = WorldSync(self, self.prospection_world) + self.pause_world_sync() self.world_sync.start() def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, @@ -359,7 +360,7 @@ def remove_object(self, obj: Object) -> None: # has a reference to the prospection world if self.prospection_world is not None: self.world_sync.remove_obj_queue.put(obj) - self.world_sync.remove_obj_queue.join() + # self.world_sync.remove_obj_queue.join() if obj.name != "floor": self.remove_object_from_simulator(obj) @@ -681,6 +682,8 @@ def exit_prospection_world_if_exists(self) -> None: Exits the prospection world if it exists. """ if self.prospection_world: + self.world_sync.empty_queues() + self.world_sync.join_queues() self.terminate_world_sync() self.prospection_world.exit() @@ -717,6 +720,7 @@ def terminate_world_sync(self) -> None: Terminates the world sync thread. """ self.world_sync.terminate = True + self.resume_world_sync() self.world_sync.join() def save_state(self, state_id: Optional[int] = None) -> int: @@ -1110,6 +1114,18 @@ def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ raise NotImplementedError + def pause_world_sync(self) -> None: + """ + Pauses the world synchronization. + """ + self.world_sync.sync_lock.acquire() + + def resume_world_sync(self) -> None: + """ + Resumes the world synchronization. + """ + self.world_sync.sync_lock.release() + def __del__(self): self.exit() @@ -1133,11 +1149,6 @@ class UseProspectionWorld: NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ - WAIT_TIME_FOR_ADDING_QUEUE = 20 - """ - The time in seconds to wait for the adding queue to be ready. - """ - def __init__(self): self.prev_world: Optional[World] = None # The previous world is saved to restore it after the with block is exited. @@ -1147,12 +1158,11 @@ def __enter__(self): This method is called when entering the with block, it will set the current world to the prospection world """ if not World.current_world.is_prospection_world: - time.sleep(self.WAIT_TIME_FOR_ADDING_QUEUE * World.current_world.simulation_time_step) # blocks until the adding queue is ready - World.current_world.world_sync.add_obj_queue.join() + World.current_world.resume_world_sync() + World.current_world.world_sync.join_queues() self.prev_world = World.current_world - World.current_world.world_sync.pause_sync = True World.current_world = World.current_world.prospection_world def __exit__(self, *args): @@ -1161,7 +1171,7 @@ def __exit__(self, *args): """ if self.prev_world is not None: World.current_world = self.prev_world - World.current_world.world_sync.pause_sync = False + World.current_world.pause_world_sync() class WorldSync(threading.Thread): @@ -1188,6 +1198,7 @@ def __init__(self, world: World, prospection_world: World): # Maps world to prospection world objects self.object_mapping: Dict[Object, Object] = {} self.equal_states = False + self.sync_lock: threading.Lock = threading.Lock() def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): """ @@ -1202,30 +1213,47 @@ def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): the syncing loop. """ while not self.terminate: - self.check_for_pause() - while not self.add_obj_queue.empty(): - obj = self.add_obj_queue.get() - # Maps the World object to the prospection world object - self.object_mapping[obj] = copy(obj) - self.add_obj_queue.task_done() - while not self.remove_obj_queue.empty(): - obj = self.remove_obj_queue.get() - # Get prospection world object reference from object mapping - prospection_obj = self.object_mapping[obj] - prospection_obj.remove() - del self.object_mapping[obj] - self.remove_obj_queue.task_done() - for world_obj, prospection_obj in self.object_mapping.items(): - prospection_obj.current_state = world_obj.current_state - self.check_for_pause() + self.sync_lock.acquire() + self.sync_worlds() + self.sync_lock.release() time.sleep(wait_time_as_n_simulation_steps * self.world.simulation_time_step) - def check_for_pause(self) -> None: + def sync_worlds(self): """ - Checks if :py:attr:`~self.pause_sync` is true and sleeps this thread until it isn't anymore. + Syncs the prospection world with the main world by adding and removing objects and synchronizing their states. """ - while self.pause_sync: - time.sleep(0.1) + self.add_queued_objects() + self.remove_queued_objects() + self.sync_objects_states() + + def add_queued_objects(self) -> None: + """ + Adds all objects in the add queue to the prospection world. + """ + while not self.add_obj_queue.empty(): + obj = self.add_obj_queue.get() + # Maps the World object to the prospection world object + self.object_mapping[obj] = copy(obj) + self.add_obj_queue.task_done() + + def remove_queued_objects(self) -> None: + """ + Removes all objects in the remove queue from the prospection world. + """ + while not self.remove_obj_queue.empty(): + obj = self.remove_obj_queue.get() + # Get prospection world object reference from object mapping + prospection_obj = self.object_mapping[obj] + prospection_obj.remove() + del self.object_mapping[obj] + self.remove_obj_queue.task_done() + + def sync_objects_states(self) -> None: + """ + Synchronizes the state of all objects in the World with the prospection world. + """ + for world_obj, prospection_obj in self.object_mapping.items(): + prospection_obj.current_state = world_obj.current_state def check_for_equal(self) -> bool: """ @@ -1239,3 +1267,33 @@ def check_for_equal(self) -> bool: eql = eql and obj.get_pose().dist(prospection_obj.get_pose()) < 0.001 self.equal_states = eql return eql + + def empty_queues(self) -> None: + """ + Empties the add and remove object queues. + """ + self.empty_add_queue() + self.empty_remove_queue() + + def empty_add_queue(self) -> None: + """ + Empties the add object queue. + """ + while not self.add_obj_queue.empty(): + self.add_obj_queue.get() + self.add_obj_queue.task_done() + + def empty_remove_queue(self) -> None: + """ + Empties the remove object queue. + """ + while not self.remove_obj_queue.empty(): + self.remove_obj_queue.get() + self.remove_obj_queue.task_done() + + def join_queues(self) -> None: + """ + Joins the add and remove object queues. + """ + self.add_obj_queue.join() + self.remove_obj_queue.join() From 06079258ac19e36a07069a5a74d913741d43434a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 16 Jul 2024 18:45:47 +0200 Subject: [PATCH 054/189] [Multiverse] updated sync to be only when UseProspectionWorld is active. --- src/pycram/__init__.py | 1 + src/pycram/datastructures/world.py | 13 ++++++-- test/test_action_designator.py | 3 +- test/test_attachment.py | 39 ++++++++++++----------- test/test_bullet_world.py | 51 ++++++++++++++++-------------- 5 files changed, 61 insertions(+), 46 deletions(-) diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index d1d5875f4..d773275cf 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -22,6 +22,7 @@ import logging.config import pycram.process_modules +import pycram.robot_descriptions logging.basicConfig(level=logging.WARNING, format='%(levelname)s - %(name)s - Line:%(lineno)d - %(message)s') diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 6bceee218..85e4f0e37 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -166,7 +166,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.let_pycram_move_attached_objects = True self.let_pycram_handle_spawning = True self.let_pycram_handle_world_sync = True - self.update_poses_from_sim_on_get = False + self.update_poses_from_sim_on_get = True if World.current_world is None: World.current_world = self @@ -360,7 +360,9 @@ def remove_object(self, obj: Object) -> None: # has a reference to the prospection world if self.prospection_world is not None: self.world_sync.remove_obj_queue.put(obj) + # self.resume_world_sync() # self.world_sync.remove_obj_queue.join() + # self.pause_world_sync() if obj.name != "floor": self.remove_object_from_simulator(obj) @@ -1148,6 +1150,7 @@ class UseProspectionWorld: with UseProspectionWorld(): NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ + WAIT_TIME_AS_N_SIMULATION_STEPS = 20 def __init__(self): self.prev_world: Optional[World] = None @@ -1158,6 +1161,7 @@ def __enter__(self): This method is called when entering the with block, it will set the current world to the prospection world """ if not World.current_world.is_prospection_world: + time.sleep(self.WAIT_TIME_AS_N_SIMULATION_STEPS * World.current_world.simulation_time_step) # blocks until the adding queue is ready World.current_world.resume_world_sync() World.current_world.world_sync.join_queues() @@ -1222,8 +1226,8 @@ def sync_worlds(self): """ Syncs the prospection world with the main world by adding and removing objects and synchronizing their states. """ - self.add_queued_objects() self.remove_queued_objects() + self.add_queued_objects() self.sync_objects_states() def add_queued_objects(self) -> None: @@ -1263,6 +1267,11 @@ def check_for_equal(self) -> bool: :return: True if both Worlds have the same state, False otherwise. """ eql = True + prospection_names = self.prospection_world.get_object_names() + eql = eql and [name in prospection_names for name in self.world.get_object_names()] + eql = eql and len(prospection_names) == len(self.world.get_object_names()) + if not eql: + return False for obj, prospection_obj in self.object_mapping.items(): eql = eql and obj.get_pose().dist(prospection_obj.get_pose()) < 0.001 self.equal_states = eql diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 9b09feac8..0d52f56aa 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -22,7 +22,8 @@ def test_move_torso(self): self.assertEqual(description.ground().position, 0.3) with simulated_robot: description.resolve().perform() - self.assertEqual(self.world.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint), 0.3) + self.assertEqual(self.world.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint), + 0.3) def test_set_gripper(self): description = action_designator.SetGripperAction([Arms.LEFT], [GripperState.OPEN, GripperState.CLOSE]) diff --git a/test/test_attachment.py b/test/test_attachment.py index d521d752a..cff5e78d6 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -52,14 +52,14 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): time.sleep(0.05) milk_2.attach(cereal_2) time.sleep(0.05) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - # self.assertTrue(cereal_2 not in prospection_milk.attachments) - prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) - # self.assertTrue(prospection_cereal in prospection_milk.attachments) - self.assertTrue(prospection_milk.attachments == {}) - - # Assert that when prospection object is moved, the real object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + # self.assertTrue(cereal_2 not in prospection_milk.attachments) + prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) + # self.assertTrue(prospection_cereal in prospection_milk.attachments) + self.assertTrue(prospection_milk.attachments == {}) + + # Assert that when prospection object is moved, the real object is not moved prospection_milk_pos = prospection_milk.get_position() cereal_pos = cereal_2.get_position() prospection_cereal_pos = prospection_cereal.get_position() @@ -68,11 +68,11 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): prospection_milk_pos.x += 1 prospection_milk.set_position(prospection_milk_pos) - # Prospection object should not move + # Prospection cereal should not move since it is not attached to prospection milk new_prospection_cereal_pose = prospection_cereal.get_position() self.assertTrue(new_prospection_cereal_pose == prospection_cereal_pos) - # Real cereal object should not move + # Also Real cereal object should not move new_cereal_pos = cereal_2.get_position() assumed_cereal_pos = cereal_pos self.assertTrue(new_cereal_pos == assumed_cereal_pos) @@ -81,20 +81,21 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): self.world.remove_object(cereal_2) def test_no_attachment_in_prospection_world(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) + milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + pose=Pose([1.3, 0.7, 0.95])) - milk_2.attach(cereal_2) + milk_2.attach(cereal_2) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) + with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) - self.assertTrue(prospection_milk.attachments == {}) - self.assertTrue(prospection_cereal.attachments == {}) + self.assertTrue(prospection_milk.attachments == {}) + self.assertTrue(prospection_cereal.attachments == {}) - self.world.remove_object(milk_2) - self.world.remove_object(cereal_2) + self.world.remove_object(milk_2) + self.world.remove_object(cereal_2) def test_attaching_to_robot_and_moving(self): self.robot.attach(self.milk) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index ec398df7a..6a379efa4 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -136,9 +136,10 @@ def test_equal_world_states(self): time.sleep(2.5) self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1])) self.assertFalse(self.world.world_sync.check_for_equal()) - self.world.prospection_world.object_states = self.world.current_state.object_states - time.sleep(0.05) - self.assertTrue(self.world.world_sync.check_for_equal()) + with UseProspectionWorld(): + self.world.prospection_world.object_states = self.world.current_state.object_states + time.sleep(0.05) + self.assertTrue(self.world.world_sync.check_for_equal()) def test_add_resource_path(self): self.world.add_resource_path("test") @@ -146,41 +147,43 @@ def test_add_resource_path(self): def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - time.sleep(0.05) try: - prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) + with UseProspectionWorld(): + prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) - time.sleep(0.1) - self.world.get_prospection_object_for_object(milk_2) + # time.sleep(0.1) + with UseProspectionWorld(): + self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) except ValueError as e: self.assertTrue(True) def test_no_object_found_for_given_prospection_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - time.sleep(0.05) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) - try: - self.world.remove_object(milk_2) - self.world.get_object_for_prospection_object(prospection_milk) - time.sleep(0.1) - self.assertFalse(True) - except ValueError as e: - self.assertTrue(True) - time.sleep(0.05) + with UseProspectionWorld(): + time.sleep(0.05) + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) + try: + self.world.remove_object(milk_2) + time.sleep(0.1) + self.world.get_object_for_prospection_object(prospection_milk) + self.assertFalse(True) + except ValueError as e: + self.assertTrue(True) + time.sleep(0.05) def test_real_object_position_does_not_change_with_prospection_object(self): milk_2_pos = [1.3, 1, 0.9] milk_2 = Object("milk_3", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_milk_pos = prospection_milk.get_position() - self.assertTrue(prospection_milk_pos == milk_2_pos) # Assert that when prospection object is moved, the real object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + prospection_milk_pos = prospection_milk.get_position() + self.assertTrue(prospection_milk_pos == milk_2_pos) prospection_milk_pos.x += 1 prospection_milk.set_position(prospection_milk_pos) self.assertTrue(prospection_milk.get_position() != milk_2.get_position()) @@ -191,12 +194,12 @@ def test_prospection_object_position_does_not_change_with_real_object(self): milk_2 = Object("milk_4", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_milk_pos = prospection_milk.get_position() - self.assertTrue(prospection_milk_pos == milk_2_pos) # Assert that when real object is moved, the prospection object is not moved with UseProspectionWorld(): + prospection_milk = self.world.get_prospection_object_for_object(milk_2) + prospection_milk_pos = prospection_milk.get_position() + self.assertTrue(prospection_milk_pos == milk_2_pos) milk_2_pos.x += 1 milk_2.set_position(milk_2_pos) self.assertTrue(prospection_milk.get_position() != milk_2.get_position()) From d2a4e6bccc687326fab678ede1061a7d250fb8e7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 17 Jul 2024 20:55:29 +0200 Subject: [PATCH 055/189] [Multiverse] Sync is only done once at the moment UseProspectionWorld() is invoked. No add or remove queues anymore. --- src/pycram/datastructures/world.py | 183 ++++++++++------------ src/pycram/world_concepts/world_object.py | 9 -- src/pycram/world_reasoning.py | 22 +-- test/test_bullet_world.py | 28 +--- test/test_bullet_world_reasoning.py | 15 +- test/test_cache_manager.py | 2 +- 6 files changed, 110 insertions(+), 149 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 85e4f0e37..8dd4f7a47 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -6,7 +6,6 @@ import time from abc import ABC, abstractmethod from copy import copy -from queue import Queue import numpy as np @@ -356,14 +355,6 @@ def remove_object(self, obj: Object) -> None: if obj.name != "floor": self.objects.remove(obj) - # This means the current world of the object is not the prospection world, since it - # has a reference to the prospection world - if self.prospection_world is not None: - self.world_sync.remove_obj_queue.put(obj) - # self.resume_world_sync() - # self.world_sync.remove_obj_queue.join() - # self.pause_world_sync() - if obj.name != "floor": self.remove_object_from_simulator(obj) @@ -684,8 +675,6 @@ def exit_prospection_world_if_exists(self) -> None: Exits the prospection world if it exists. """ if self.prospection_world: - self.world_sync.empty_queues() - self.world_sync.join_queues() self.terminate_world_sync() self.prospection_world.exit() @@ -855,16 +844,15 @@ def get_prospection_object_for_object(self, obj: Object) -> Object: :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. """ - self.world_sync.add_obj_queue.join() - try: - return self.world_sync.object_mapping[obj] - except KeyError: - prospection_world = self if self.is_prospection_world else self.prospection_world - if obj in prospection_world.objects: - return obj - else: - raise ValueError( - f"There is no prospection object for the given object: {obj}, this could be the case if" + with UseProspectionWorld(): + try: + if obj.world.is_prospection_world: + return obj + else: + return self.world_sync.get_prospection_object(obj) + except KeyError: + raise KeyError( + f"There is no prospection object for the given object: {obj.name}, this could be the case if" f" the object isn't anymore in the main (graphical) World" f" or if the given object is already a prospection object. ") @@ -877,19 +865,23 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. """ - object_map = self.world_sync.object_mapping - try: - return list(object_map.keys())[list(object_map.values()).index(prospection_object)] - except ValueError: - raise ValueError("The given object is not in the prospection world.") + with UseProspectionWorld(): + try: + if not prospection_object.world.is_prospection_world: + return prospection_object + else: + return self.world_sync.get_world_object(prospection_object) + except KeyError: + raise KeyError(f"The given object {prospection_object.name} is not in the prospection world.") - def reset_world_and_remove_objects(self) -> None: + def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]]) -> None: """ Resets the World to the state it was first spawned in and removes all objects from the World. + :param exclude_objects: A list of objects that should not be removed. """ self.reset_world() - for obj in copy(self.objects): - self.remove_object(obj) + objs_copy = copy(self.objects) + [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] def reset_world(self, remove_saved_states=True) -> None: """ @@ -1151,6 +1143,9 @@ class UseProspectionWorld: NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ WAIT_TIME_AS_N_SIMULATION_STEPS = 20 + """ + The time in simulation steps to wait before switching to the prospection world + """ def __init__(self): self.prev_world: Optional[World] = None @@ -1161,13 +1156,11 @@ def __enter__(self): This method is called when entering the with block, it will set the current world to the prospection world """ if not World.current_world.is_prospection_world: - time.sleep(self.WAIT_TIME_AS_N_SIMULATION_STEPS * World.current_world.simulation_time_step) - # blocks until the adding queue is ready - World.current_world.resume_world_sync() - World.current_world.world_sync.join_queues() - self.prev_world = World.current_world World.current_world = World.current_world.prospection_world + World.current_world.resume_world_sync() + time.sleep(self.WAIT_TIME_AS_N_SIMULATION_STEPS * World.current_world.simulation_time_step) + World.current_world.pause_world_sync() def __exit__(self, *args): """ @@ -1175,7 +1168,6 @@ def __exit__(self, *args): """ if self.prev_world is not None: World.current_world = self.prev_world - World.current_world.pause_world_sync() class WorldSync(threading.Thread): @@ -1183,12 +1175,15 @@ class WorldSync(threading.Thread): Synchronizes the state between the World and its prospection world. Meaning the cartesian and joint position of everything in the prospection world will be synchronized with the main World. - Adding and removing objects is done via queues, such that loading times of objects - in the prospection world does not affect the World. The class provides the possibility to pause the synchronization, this can be used if reasoning should be done in the prospection world. """ + WAIT_TIME_AS_N_SIMULATION_STEPS = 20 + """ + The time in simulation steps to wait between each iteration of the syncing loop. + """ + def __init__(self, world: World, prospection_world: World): threading.Thread.__init__(self) self.world: World = world @@ -1196,67 +1191,91 @@ def __init__(self, world: World, prospection_world: World): self.prospection_world.world_sync = self self.terminate: bool = False - self.add_obj_queue: Queue = Queue() - self.remove_obj_queue: Queue = Queue() self.pause_sync: bool = False # Maps world to prospection world objects - self.object_mapping: Dict[Object, Object] = {} + self.object_to_prospection_object_map: Dict[Object, Object] = {} + self.prospection_object_to_object_map: Dict[Object, Object] = {} self.equal_states = False self.sync_lock: threading.Lock = threading.Lock() - def run(self, wait_time_as_n_simulation_steps: Optional[int] = 1): + def run(self): """ Main method of the synchronization, this thread runs in a loop until the terminate flag is set. While this loop runs it continuously checks the cartesian and joint position of every object in the World and updates the corresponding object in the - prospection world. When there are entries in the adding or removing queue the corresponding objects will - be added or removed in the same iteration. - - :param wait_time_as_n_simulation_steps: The time in simulation steps to wait between each iteration of - the syncing loop. + prospection world. """ while not self.terminate: self.sync_lock.acquire() self.sync_worlds() self.sync_lock.release() - time.sleep(wait_time_as_n_simulation_steps * self.world.simulation_time_step) + time.sleep(WorldSync.WAIT_TIME_AS_N_SIMULATION_STEPS * self.world.simulation_time_step) + + def get_world_object(self, prospection_object: Object) -> Object: + """ + Returns the corresponding object from the main World for a given object in the prospection world. + + :param prospection_object: The object for which the corresponding object in the main World should be found. + :return: The object in the main World. + """ + return self.prospection_object_to_object_map[prospection_object] + + def get_prospection_object(self, obj: Object) -> Object: + """ + Returns the corresponding object from the prospection world for a given object in the main world. + + :param obj: The object for which the corresponding object in the prospection World should be found. + :return: The corresponding object in the prospection world. + """ + return self.object_to_prospection_object_map[obj] def sync_worlds(self): """ Syncs the prospection world with the main world by adding and removing objects and synchronizing their states. """ - self.remove_queued_objects() - self.add_queued_objects() + self.remove_objects_not_in_world() + self.add_objects_not_in_prospection_world() + self.prospection_object_to_object_map = {prospection_obj: obj for obj, prospection_obj in + self.object_to_prospection_object_map.items()} self.sync_objects_states() - def add_queued_objects(self) -> None: + def remove_objects_not_in_world(self): + """ + Removes all objects that are not in the main world from the prospection world. + """ + obj_map_copy = copy(self.object_to_prospection_object_map) + [self.remove_object(obj) for obj in obj_map_copy.keys() if obj not in self.world.objects] + + def add_objects_not_in_prospection_world(self): """ - Adds all objects in the add queue to the prospection world. + Adds all objects that are in the main world but not in the prospection world to the prospection world. """ - while not self.add_obj_queue.empty(): - obj = self.add_obj_queue.get() - # Maps the World object to the prospection world object - self.object_mapping[obj] = copy(obj) - self.add_obj_queue.task_done() + [self.add_object(obj) for obj in self.world.objects if obj not in self.object_to_prospection_object_map] - def remove_queued_objects(self) -> None: + def add_object(self, obj: Object) -> None: """ - Removes all objects in the remove queue from the prospection world. + Adds an object to the prospection world. + + :param obj: The object to be added. + """ + self.object_to_prospection_object_map[obj] = copy(obj) + + def remove_object(self, obj: Object) -> None: + """ + Removes an object from the prospection world. + + :param obj: The object to be removed. """ - while not self.remove_obj_queue.empty(): - obj = self.remove_obj_queue.get() - # Get prospection world object reference from object mapping - prospection_obj = self.object_mapping[obj] - prospection_obj.remove() - del self.object_mapping[obj] - self.remove_obj_queue.task_done() + prospection_obj = self.object_to_prospection_object_map[obj] + prospection_obj.remove() + del self.object_to_prospection_object_map[obj] def sync_objects_states(self) -> None: """ Synchronizes the state of all objects in the World with the prospection world. """ - for world_obj, prospection_obj in self.object_mapping.items(): + for world_obj, prospection_obj in self.object_to_prospection_object_map.items(): prospection_obj.current_state = world_obj.current_state def check_for_equal(self) -> bool: @@ -1272,37 +1291,7 @@ def check_for_equal(self) -> bool: eql = eql and len(prospection_names) == len(self.world.get_object_names()) if not eql: return False - for obj, prospection_obj in self.object_mapping.items(): + for obj, prospection_obj in self.object_to_prospection_object_map.items(): eql = eql and obj.get_pose().dist(prospection_obj.get_pose()) < 0.001 self.equal_states = eql return eql - - def empty_queues(self) -> None: - """ - Empties the add and remove object queues. - """ - self.empty_add_queue() - self.empty_remove_queue() - - def empty_add_queue(self) -> None: - """ - Empties the add object queue. - """ - while not self.add_obj_queue.empty(): - self.add_obj_queue.get() - self.add_obj_queue.task_done() - - def empty_remove_queue(self) -> None: - """ - Empties the remove object queue. - """ - while not self.remove_obj_queue.empty(): - self.remove_obj_queue.get() - self.remove_obj_queue.task_done() - - def join_queues(self) -> None: - """ - Joins the add and remove object queues. - """ - self.add_obj_queue.join() - self.remove_obj_queue.join() diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 59912a269..410651bfe 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -87,9 +87,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.attachments: Dict[Object, Attachment] = {} - if not self.world.is_prospection_world: - self._add_to_world_sync_obj_queue() - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: rdm = RobotDescriptionManager() rdm.load_description(self.name) @@ -184,12 +181,6 @@ def _init_joints(self): joint_description = self.description.get_joint_by_name(joint_name) self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) - def _add_to_world_sync_obj_queue(self) -> None: - """ - Adds this object to the objects queue of the WorldSync object of the World. - """ - self.world.world_sync.add_obj_queue.put(self) - @property def has_one_link(self) -> bool: """ diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index e52c48f39..1124cabf8 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -1,14 +1,14 @@ import itertools -from typing_extensions import List, Tuple, Optional, Union, Dict import numpy as np +from typing_extensions import List, Tuple, Optional, Union, Dict -from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp +from .datastructures.dataclasses import ContactPointsList from .datastructures.pose import Pose, Transform +from .datastructures.world import World, UseProspectionWorld +from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .robot_description import RobotDescription -from .datastructures.dataclasses import ContactPointsList from .world_concepts.world_object import Object -from .datastructures.world import World, UseProspectionWorld def stable(obj: Object) -> bool: @@ -51,17 +51,13 @@ def contact( with UseProspectionWorld(): prospection_obj1 = World.current_world.get_prospection_object_for_object(object1) prospection_obj2 = World.current_world.get_prospection_object_for_object(object2) - World.current_world.perform_collision_detection() con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1, prospection_obj2) objects_are_in_contact = len(con_points) > 0 if return_links: - contact_links = [] - for point in con_points: - contact_links.append((point.link_a, point.link_b)) + contact_links = [(point.link_a, point.link_b) for point in con_points] return objects_are_in_contact, contact_links - else: return objects_are_in_contact @@ -227,17 +223,15 @@ def blocking( :return: A list of objects the robot is in collision with when reaching for the specified object or None if the pose or object is not reachable. """ - prospection_robot = World.current_world.get_prospection_object_for_object(robot) with UseProspectionWorld(): + prospection_robot = World.current_world.get_prospection_object_for_object(robot) if grasp: try_to_reach_with_grasp(pose_or_object, prospection_robot, gripper_name, grasp) else: try_to_reach(pose_or_object, prospection_robot, gripper_name) - block = [] - for obj in World.current_world.objects: - if contact(prospection_robot, obj): - block.append(World.current_world.get_object_for_prospection_object(obj)) + block = [World.current_world.get_object_for_prospection_object(obj) for obj in World.current_world.objects + if contact(prospection_robot, obj)] return block diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 6a379efa4..05cf36cff 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -11,7 +11,7 @@ from pycram.object_descriptors.urdf import ObjectDescription from pycram.datastructures.dataclasses import Color from pycram.world_concepts.world_object import Object -from pycram.datastructures.world import UseProspectionWorld +from pycram.datastructures.world import UseProspectionWorld, World fix_missing_inertial = ObjectDescription.fix_missing_inertial @@ -137,8 +137,6 @@ def test_equal_world_states(self): self.robot.set_pose(Pose([1, 0, 0], [0, 0, 0, 1])) self.assertFalse(self.world.world_sync.check_for_equal()) with UseProspectionWorld(): - self.world.prospection_world.object_states = self.world.current_state.object_states - time.sleep(0.05) self.assertTrue(self.world.world_sync.check_for_equal()) def test_add_resource_path(self): @@ -148,31 +146,13 @@ def test_add_resource_path(self): def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) try: - with UseProspectionWorld(): - prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) + prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) - # time.sleep(0.1) - with UseProspectionWorld(): - self.world.get_prospection_object_for_object(milk_2) + self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) - except ValueError as e: + except KeyError as e: self.assertTrue(True) - def test_no_object_found_for_given_prospection_object(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - with UseProspectionWorld(): - time.sleep(0.05) - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - self.assertTrue(self.world.get_object_for_prospection_object(prospection_milk) == milk_2) - try: - self.world.remove_object(milk_2) - time.sleep(0.1) - self.world.get_object_for_prospection_object(prospection_milk) - self.assertFalse(True) - except ValueError as e: - self.assertTrue(True) - time.sleep(0.05) - def test_real_object_position_does_not_change_with_prospection_object(self): milk_2_pos = [1.3, 1, 0.9] milk_2 = Object("milk_3", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 3fafe27d4..fe9998228 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -28,20 +28,27 @@ def test_visible(self): def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) - self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()), + self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose( + RobotDescription.current_robot_description.get_camera_frame()), RobotDescription.current_robot_description.get_default_camera().front_facing_axis) != []) def test_reachable(self): self.robot.set_pose(Pose()) time.sleep(1) - self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame())) - self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame())) + self.assertTrue(btr.reachable(Pose([0.5, -0.7, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains[ + "right"].get_tool_frame())) + self.assertFalse(btr.reachable(Pose([2, 2, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains[ + "right"].get_tool_frame())) def test_blocking(self): self.milk.set_pose(Pose([0.5, -0.7, 1])) self.robot.set_pose(Pose()) time.sleep(2) - self.assertTrue(btr.blocking(Pose([0.5, -0.7, 1]), self.robot, RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame()) != []) + blocking = btr.blocking(Pose([0.5, -0.7, 1]), self.robot, + RobotDescription.current_robot_description.kinematic_chains["right"].get_tool_frame()) + self.assertTrue(blocking != []) def test_supporting(self): self.milk.set_pose(Pose([1.3, 0, 0.9])) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index bad803f22..95549d85e 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -13,7 +13,7 @@ def test_generate_description_and_write_to_cache(self): file_path = pathlib.Path(__file__).parent.resolve() path = str(file_path) + "/../resources/apartment.urdf" extension = Path(path).suffix - cache_path = self.world.cache_dir + "apartment.urdf" + cache_path = self.world.cache_dir + "/apartment.urdf" apartment = Object("apartment", ObjectType.ENVIRONMENT, path) cache_manager.generate_description_and_write_to_cache(path, apartment.name, extension, cache_path, apartment.description) From 998075255a857f3beab0aae943e30526f92b346a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 18 Jul 2024 19:00:08 +0200 Subject: [PATCH 056/189] [Multiverse] set mobile robot pose now working. Added the virtual joints to the robot description at initialization of the Object. --- src/pycram/cache_manager.py | 15 +++++++ src/pycram/datastructures/dataclasses.py | 16 ++++++++ src/pycram/datastructures/world.py | 9 ++++- src/pycram/description.py | 26 ++++++++++--- src/pycram/object_descriptors/urdf.py | 26 ++++++++++++- .../robot_descriptions/tiago_description.py | 2 +- .../world_concepts/multiverse_clients.py | 39 ++++++++++++++++--- src/pycram/world_concepts/world_object.py | 24 ++++++++---- src/pycram/worlds/multiverse.py | 30 +++++++++----- test/test_multiverse.py | 1 + 10 files changed, 156 insertions(+), 32 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 3cc0c254c..7c3e73913 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -1,6 +1,7 @@ import glob import os import pathlib +import shutil from typing_extensions import List, TYPE_CHECKING @@ -29,6 +30,20 @@ def __init__(self, cache_dir: str, data_directory: List[str]): self.cache_dir = cache_dir self.data_directory = data_directory + def clear_cache(self): + """ + Clears the cache directory. + """ + self.delete_cache_dir() + self.create_cache_dir_if_not_exists() + + def delete_cache_dir(self): + """ + Deletes the cache directory. + """ + if pathlib.Path(self.cache_dir).exists(): + shutil.rmtree(self.cache_dir) + def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, object_description: 'ObjectDescription', object_name: str) -> str: """ diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index e4d0e9350..d4de5b910 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -427,3 +427,19 @@ class VirtualMoveBaseJoints: translation_x: str translation_y: str angular_z: str + child_link: str + + def as_list(self) -> List[str]: + return [self.translation_x, self.translation_y, self.angular_z] + + def get_types(self) -> Dict[str, JointType]: + return {self.translation_x: JointType.PRISMATIC, + self.translation_y: JointType.PRISMATIC, + self.angular_z: JointType.REVOLUTE} + + def get_axes(self) -> Dict[str, Point]: + return {self.translation_x: Point(1, 0, 0), + self.translation_y: Point(0, 1, 0), + self.angular_z: Point(0, 0, 1)} + + diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 8dd4f7a47..1f18fa787 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -145,6 +145,11 @@ class World(StateEntity, ABC): Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ + cache_manager: CacheManager = CacheManager(cache_dir, data_directory) + """ + Global reference for the cache manager, this is used to cache the description files of the robot and the objects. + """ + prospection_world_prefix: str = "prospection_" """ The prefix for the prospection world name. @@ -171,7 +176,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ World.current_world = self World.simulation_frequency = simulation_frequency - self.cache_manager = CacheManager(self.cache_dir, self.data_directory) self.object_lock: threading.Lock = threading.Lock() self.id: Optional[int] = -1 @@ -874,13 +878,14 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec except KeyError: raise KeyError(f"The given object {prospection_object.name} is not in the prospection world.") - def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]]) -> None: + def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: """ Resets the World to the state it was first spawned in and removes all objects from the World. :param exclude_objects: A list of objects that should not be removed. """ self.reset_world() objs_copy = copy(self.objects) + exclude_objects = [] if exclude_objects is None else exclude_objects [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] def reset_world(self, remove_saved_states=True) -> None: diff --git a/src/pycram/description.py b/src/pycram/description.py index bdec4fafa..f4c1d78e3 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -8,18 +8,17 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING +from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape from .datastructures.enums import JointType -from .local_transformer import LocalTransformer from .datastructures.pose import Pose, Transform from .datastructures.world import WorldEntity -from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from .local_transformer import LocalTransformer if TYPE_CHECKING: from .world_concepts.world_object import Object class EntityDescription(ABC): - """ A class that represents a description of an entity. This can be a link, joint or object description. """ @@ -63,7 +62,7 @@ class JointDescription(EntityDescription): A class that represents the description of a joint. """ - def __init__(self, parsed_joint_description: Any): + def __init__(self, parsed_joint_description: Optional[Any] = None): self.parsed_description = parsed_joint_description @property @@ -560,7 +559,6 @@ def __hash__(self): class ObjectDescription(EntityDescription): - """ A class that represents the description of an object. """ @@ -588,6 +586,24 @@ def __init__(self, path: Optional[str] = None): else: self._parsed_description = None + @abstractmethod + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + """ + Adds a joint to this object. + + :param name: The name of the joint. + :param child: The name of the child link. + :param joint_type: The type of the joint. + :param axis: The axis of the joint. + :param parent: The name of the parent link. + :param origin: The origin of the joint. + :param lower_limit: The lower limit of the joint. + :param upper_limit: The upper limit of the joint. + """ + pass + def update_description_from_file(self, path: str) -> None: """ Updates the description of this object from the file at the given path. diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index ac33a481d..7288df776 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -4,7 +4,7 @@ import rospkg import rospy from geometry_msgs.msg import Point -from tf.transformations import quaternion_from_euler +from tf.transformations import quaternion_from_euler, euler_from_quaternion from typing_extensions import Union, List, Optional from urdf_parser_py import urdf from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, @@ -79,6 +79,8 @@ class JointDescription(AbstractJointDescription): 'planar': JointType.PLANAR, 'fixed': JointType.FIXED} + pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} + def __init__(self, urdf_description: urdf.Joint): super().__init__(urdf_description) @@ -172,6 +174,28 @@ class RootLink(AbstractObjectDescription.RootLink, Link): class Joint(AbstractObjectDescription.Joint, JointDescription): ... + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + if lower_limit is not None or upper_limit is not None: + limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) + else: + limit = None + if origin is not None: + origin = urdf.Pose(origin.position_as_list(), euler_from_quaternion(origin.orientation_as_list())) + if axis is not None: + axis = [axis.x, axis.y, axis.z] + if parent is None: + parent = self.get_root() + else: + parent = self.get_link_by_name(parent).parsed_description + joint = urdf.Joint(name, + parent, + self.get_link_by_name(child).parsed_description, + JointDescription.pycram_type_map[joint_type], + axis, origin, limit) + self.parsed_description.add_joint(joint) + def load_description(self, path) -> URDF: with open(path, 'r') as file: # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index 0c834694c..f0df89728 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -10,7 +10,7 @@ tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", filename, VirtualMoveBaseJoints("odom_vel_lin_x_joint", "odom_vel_lin_y_joint", - "odom_vel_ang_z_joint")) + "odom_vel_ang_z_joint", "base_link")) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "torso_lift_link", "arm_left_7_link", diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index 9fa8b75cb..314c91901 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -64,6 +64,15 @@ class MultiverseContactPoint: class MultiverseReader(MultiverseSocket): + + PORT: int = 9000 + """ + The port of the Multiverse reader client. + """ + PROSPECTION_PORT: int = PORT + 3 + """ + The port of the Multiverse reader client for the prospection world.""" + def __init__(self, max_wait_time_for_data: Optional[float] = 1, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. @@ -75,8 +84,8 @@ def __init__(self, max_wait_time_for_data: Optional[float] = 1, is_prospection_w meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "reader" meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = "8003" if is_prospection_world else "8000" - super().__init__(SocketAddress(port=port), meta_data) + port = self.PROSPECTION_PORT if is_prospection_world else self.PORT + super().__init__(SocketAddress(port=str(port)), meta_data) self.run() self.data_lock = threading.Lock() self.thread = threading.Thread(target=self.receive_all_data_from_server) @@ -211,6 +220,15 @@ class MultiverseWriter(MultiverseSocket): Wait time for the sent data to be applied in the simulator. """ + PORT: int = MultiverseReader.PORT + 1 + """ + The port of the Multiverse writer client. + """ + PROSPECTION_PORT: int = PORT + 3 + """ + The port of the Multiverse writer client for the prospection world. + """ + def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. @@ -222,8 +240,8 @@ def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "writer" meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = "8004" if is_prospection_world else "8001" - super().__init__(SocketAddress(port=port), meta_data) + port = self.PROSPECTION_PORT if is_prospection_world else self.PORT + super().__init__(SocketAddress(port=str(port)), meta_data) self.simulation = simulation self.time_start = time() self.run() @@ -351,6 +369,15 @@ class MultiverseAPI(MultiverseSocket): The API names for the API callbacks to the Multiverse server. """ + PORT: int = MultiverseWriter.PORT + 1 + """ + The port of the Multiverse API client. + """ + PROSPECTION_PORT: int = PORT + 3 + """ + The port of the Multiverse API client for the prospection world. + """ + def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. @@ -362,8 +389,8 @@ def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "api_requester" meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = "8005" if is_prospection_world else "8002" - super().__init__(SocketAddress(port=port), meta_data) + port = self.PROSPECTION_PORT if is_prospection_world else self.PORT + super().__init__(SocketAddress(port=str(port)), meta_data) self.simulation = simulation self.run() diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 410651bfe..6ba34659d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,7 +8,7 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint +from ..description import ObjectDescription, LinkDescription, Joint, JointDescription from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..datastructures.world import WorldEntity, World from ..world_concepts.constraints import Attachment @@ -18,7 +18,7 @@ from ..datastructures.enums import ObjectType, JointType from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose, Transform -from ..robot_description import RobotDescriptionManager +from ..robot_description import RobotDescriptionManager, RobotDescription Link = ObjectDescription.Link @@ -77,6 +77,12 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.description.update_description_from_file(self.path) + if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: + rdm = RobotDescriptionManager() + rdm.load_description(self.name) + World.robot = self + self._add_virtual_move_base_joints() + self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name self._init_joint_name_and_id_map() @@ -87,11 +93,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.attachments: Dict[Object, Attachment] = {} - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - rdm = RobotDescriptionManager() - rdm.load_description(self.name) - World.robot = self - self.world.objects.append(self) @property @@ -129,7 +130,7 @@ def _load_object_and_get_id(self, path: str, try: path = self.path if self.world.let_pycram_handle_spawning else self.name - obj_id = self.world.load_object_and_get_id(path, self._current_pose) + obj_id = self.world.load_object_and_get_id(path, self._current_pose, self.obj_type) return obj_id, self.path except Exception as e: @@ -139,6 +140,13 @@ def _load_object_and_get_id(self, path: str, os.remove(path) raise e + def _add_virtual_move_base_joints(self): + virtual_joints = RobotDescription.current_robot_description.virtual_move_base_joints + child_link = self.root_link_name + axes = virtual_joints.get_axes() + for joint_name, joint_type in virtual_joints.get_types().items(): + self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name]) + def _init_joint_name_and_id_map(self) -> None: """ Creates a dictionary which maps the joint names to their unique ids and vice versa. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 07bb28389..a16a92712 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -99,9 +99,11 @@ def _make_sure_multiverse_resources_are_added(self): Add the multiverse resources to the pycram world resources. """ if not self.added_multiverse_resources: + World.cache_manager.clear_cache() dirname = find_multiverse_resources_path() resources_paths = get_resource_paths(dirname) World.data_directory = resources_paths + self.data_directory + World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True def _spawn_floor(self): @@ -148,7 +150,7 @@ def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: def get_object_joint_names(self, obj: Object) -> List[str]: return [joint.name for joint in obj.description.joints - if joint.type in self._joint_type_to_position_name.keys()] + if joint.type in self._joint_type_to_position_name.keys()] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] @@ -167,8 +169,12 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: data = {joint.name: {self.get_joint_position_name(joint): [joint_poses[joint.name]]} - for joint in obj.joints.values()} - self.writer.send_multiple_body_data_to_server(data) + for joint in obj.joints.values() if joint.name in joint_poses.keys()} + if len(data) > 0: + self.writer.send_multiple_body_data_to_server(data) + else: + logging.warning(f"No joints found in object {obj.name}") + raise ValueError(f"No joints found in object {obj.name}") def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]): self.writer.send_body_data_to_server(body_name, data) @@ -198,7 +204,8 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return - elif obj.obj_type == ObjectType.ROBOT and RobotDescription.virtual_move_base_joints is not None: + elif (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_move_base_joints is not None): self.set_mobile_robot_pose(obj, pose) else: self._set_body_pose(obj.name, pose) @@ -209,11 +216,16 @@ def set_mobile_robot_pose(self, robot: Object, pose: Pose): param robot: The robot object. param pose: The pose of the robot. """ + current_position = robot.get_position() + position_diff = [pose.position.x - current_position.x, + pose.position.y - current_position.y] + current_angle = euler_from_quaternion(robot.get_orientation_as_list())[2] + angle_diff = euler_from_quaternion(pose.orientation_as_list())[2] - current_angle # Get the joints of the base link - base_link_joints: VirtualMoveBaseJoints = RobotDescription.virtual_move_base_joints - joint_name_position_dict = {base_link_joints.translation_x: pose.position.x, - base_link_joints.translation_y: pose.position.y, - base_link_joints.angular_z: euler_from_quaternion(pose.orientation_as_list())[2]} + base_link_joints: VirtualMoveBaseJoints = RobotDescription.current_robot_description.virtual_move_base_joints + joint_name_position_dict = {base_link_joints.translation_x: position_diff[0], + base_link_joints.translation_y: position_diff[1], + base_link_joints.angular_z: angle_diff} # get the pose in the joint frame self.set_multiple_joint_positions(robot, joint_name_position_dict) @@ -461,4 +473,4 @@ def find_multiverse_path() -> Optional[str]: multiverse_path = path.split('multiverse')[0] return multiverse_path + 'multiverse' - return None \ No newline at end of file + return None diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5b872916e..386581910 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -159,6 +159,7 @@ def test_respawn_robot(self): def test_set_robot_position(self): self.spawn_robot(robot_name='tiago_dual') new_position = [-3, -3, 0.001] + # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) From 35494fbcb68d19d427c0e6cabc9678f6ec72ad22 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 19 Jul 2024 12:12:49 +0200 Subject: [PATCH 057/189] [Multiverse] coincide attachment in progress. --- src/pycram/datastructures/dataclasses.py | 48 ++++++++++-- src/pycram/datastructures/enums.py | 9 +++ src/pycram/datastructures/world.py | 77 +++++++++++++++++-- src/pycram/description.py | 9 ++- src/pycram/robot_description.py | 10 +++ .../robot_descriptions/tiago_description.py | 5 +- src/pycram/world_concepts/constraints.py | 7 +- src/pycram/world_concepts/world_object.py | 27 +++++-- src/pycram/worlds/multiverse.py | 21 +---- test/test_multiverse.py | 16 ++-- 10 files changed, 174 insertions(+), 55 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d4de5b910..44e1ac0a8 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -2,7 +2,7 @@ from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from .enums import JointType, Shape +from .enums import JointType, Shape, VirtualMoveBaseJointNames from .pose import Pose, Point from abc import ABC, abstractmethod @@ -271,21 +271,33 @@ def visual_geometry_type(self) -> Shape: @dataclass class State(ABC): + """ + Abstract dataclass for storing the state of an entity (e.g. world, object, link, joint). + """ pass @dataclass class LinkState(State): + """ + Dataclass for storing the state of a link. + """ constraint_ids: Dict[Link, int] @dataclass class JointState(State): + """ + Dataclass for storing the state of a joint. + """ position: float @dataclass class ObjectState(State): + """ + Dataclass for storing the state of an object. + """ pose: Pose attachments: Dict[Object, Attachment] link_states: Dict[int, LinkState] @@ -294,18 +306,27 @@ class ObjectState(State): @dataclass class WorldState(State): + """ + Dataclass for storing the state of the world. + """ simulator_state_id: int object_states: Dict[str, ObjectState] @dataclass class LateralFriction: + """ + Dataclass for storing the information of the lateral friction. + """ lateral_friction: float lateral_friction_direction: List[float] @dataclass class ContactPoint: + """ + Dataclass for storing the information of a contact point between two objects. + """ link_a: Link link_b: Link position_on_object_a: Optional[List[float]] = None @@ -330,6 +351,9 @@ def __repr__(self): class ContactPointsList(list): + """ + A list of contact points. + """ def __index__(self, index: int) -> ContactPoint: return super().__getitem__(index) @@ -416,6 +440,9 @@ def __repr__(self): @dataclass class TextAnnotation: + """ + Dataclass for storing text annotations that can be displayed in the simulation. + """ text: str position: List[float] color: Color @@ -424,20 +451,25 @@ class TextAnnotation: @dataclass class VirtualMoveBaseJoints: - translation_x: str - translation_y: str - angular_z: str - child_link: str - - def as_list(self) -> List[str]: - return [self.translation_x, self.translation_y, self.angular_z] + """ + Dataclass for storing the names, types and axes of the virtual move base joints of a mobile robot. + """ + translation_x: Optional[str] = VirtualMoveBaseJointNames.LINEAR_X.value + translation_y: Optional[str] = VirtualMoveBaseJointNames.LINEAR_Y.value + angular_z: Optional[str] = VirtualMoveBaseJointNames.ANGULAR_Z.value def get_types(self) -> Dict[str, JointType]: + """ + Returns the joint types of the virtual move base joints. + """ return {self.translation_x: JointType.PRISMATIC, self.translation_y: JointType.PRISMATIC, self.angular_z: JointType.REVOLUTE} def get_axes(self) -> Dict[str, Point]: + """ + Returns the axes (i.e. The axis on which the joint moves) of the virtual move base joints. + """ return {self.translation_x: Point(1, 0, 0), self.translation_y: Point(0, 1, 0), self.angular_z: Point(0, 0, 1)} diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 301263473..03e321cb3 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -120,3 +120,12 @@ class GripperType(Enum): CUSTOM = auto() +class VirtualMoveBaseJointNames(Enum): + """ + Enum for the joint names of the virtual move base. + """ + LINEAR_X = "odom_vel_lin_x_joint" + LINEAR_Y = "odom_vel_lin_y_joint" + ANGULAR_Z = "odom_vel_ang_z_joint" + + diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 1f18fa787..ec061d7b3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -11,18 +11,21 @@ import numpy as np import rospy from geometry_msgs.msg import Point +from tf.transformations import euler_from_quaternion from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING from typing_extensions import Union from ..cache_manager import CacheManager from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, - MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, - CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState, ClosestPointsList, - ContactPointsList) -from ..datastructures.enums import JointType, ObjectType, WorldMode + MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, + SphereVisualShape, + CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, + ObjectState, State, WorldState, ClosestPointsList, + ContactPointsList, VirtualMoveBaseJoints) +from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform from ..local_transformer import LocalTransformer +from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event @@ -486,6 +489,60 @@ def get_object_pose(self, obj: Object) -> Pose: """ pass + def set_mobile_robot_pose(self, pose: Pose) -> None: + """ + Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when + the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). + param pose: The target pose. + """ + goal = self.get_move_base_joint_goal(pose) + self.robot.set_joint_positions(goal) + + def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + """ + Get the goal for the move base joints of a mobile robot to reach a target pose. + param pose: The target pose. + return: The goal for the move base joints. + """ + position_diff = self.get_position_diff(self.robot.get_position_as_list(), pose.position_as_list())[:2] + angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) + # Get the joints of the base link + move_base_joints = self.get_move_base_joints() + return {move_base_joints.translation_x: position_diff[0], + move_base_joints.translation_y: position_diff[1], + move_base_joints.angular_z: angle_diff} + + @staticmethod + def get_move_base_joints() -> VirtualMoveBaseJoints: + """ + Get the move base joints of the robot. + + :return: The move base joints. + """ + return RobotDescription.current_robot_description.virtual_move_base_joints + + @staticmethod + def get_position_diff(current_position: List[float], target_position: List[float]) -> List[float]: + """ + Get the difference between two positions. + param current_position: The current position. + param target_position: The target position. + return: The difference between the two positions. + """ + return [target_position[i] - current_position[i] for i in range(3)] + + @staticmethod + def get_z_angle_diff(current_quaternion: List[float], target_quaternion: List[float]) -> float: + """ + Get the difference between the z angles of two quaternions. + param current_quaternion: The current quaternion. + param target_quaternion: The target quaternion. + return: The difference between the z angles of the two quaternions in euler angles. + """ + current_angle = euler_from_quaternion(current_quaternion)[2] + target_angle = euler_from_quaternion(target_quaternion)[2] + return target_angle - current_angle + @abstractmethod def perform_collision_detection(self) -> None: """ @@ -567,6 +624,16 @@ def step(self): """ pass + def get_arm_tool_frame_link(self, arm: Arms) -> Link: + """ + Get the tool frame link of the arm of the robot. + + :param arm: The arm for which the tool frame link should be returned. + :return: The tool frame link of the arm. + """ + ee_link_name = RobotDescription.current_robot_description.get_arm_tool_frame(arm) + return self.robot.get_link(ee_link_name) + @abstractmethod def set_link_color(self, link: Link, rgba_color: Color): """ diff --git a/src/pycram/description.py b/src/pycram/description.py index f4c1d78e3..1043ffac4 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -213,16 +213,17 @@ def current_state(self) -> LinkState: def current_state(self, link_state: LinkState) -> None: self.constraint_ids = link_state.constraint_ids - def add_fixed_constraint_with_link(self, child_link: 'Link') -> int: + def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional[Transform] = None) -> int: """ Adds a fixed constraint between this link and the given link, used to create attachments for example. :param child_link: The child link to which a fixed constraint should be added. + :param transform: The transformation between the two links. :return: The unique id of the constraint. """ - constraint_id = self.world.add_fixed_constraint(self, - child_link, - child_link.get_transform_from_link(self)) + if transform is None: + transform = child_link.get_transform_from_link(self) + constraint_id = self.world.add_fixed_constraint(self, child_link, transform) self.constraint_ids[child_link] = constraint_id child_link.constraint_ids[self] = constraint_id return constraint_id diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 35274edda..c6b77a67b 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -301,6 +301,16 @@ def get_child(self, name: str, return_multiple_children: bool = False) -> Union[ child_link = self.urdf_object.joint_map[name].child return child_link + def get_arm_tool_frame(self, arm: Arms) -> str: + """ + Returns the name of the tool frame of a specific arm. + + :param arm: Arm for which the tool frame should be returned + :return: The name of the link of the tool frame in the URDF. + """ + chain = self.get_arm_chain(arm) + return chain.get_tool_frame() + def get_arm_chain(self, arm: Arms) -> Union[KinematicChainDescription, List[KinematicChainDescription]]: """ Returns the kinematic chain of a specific arm. If the arm is set to BOTH, all kinematic chains are returned. diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index f0df89728..c5c7814d9 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -1,16 +1,15 @@ import rospkg from ..datastructures.dataclasses import VirtualMoveBaseJoints +from ..datastructures.enums import GripperState, Arms, Grasp from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..datastructures.enums import GripperState, Arms, Grasp rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", - filename, VirtualMoveBaseJoints("odom_vel_lin_x_joint", "odom_vel_lin_y_joint", - "odom_vel_ang_z_joint", "base_link")) + filename, VirtualMoveBaseJoints()) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "torso_lift_link", "arm_left_7_link", diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index aa41e3bdf..ff4514e0d 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -181,7 +181,10 @@ def __init__(self, self.bidirectional: bool = bidirectional self._loose: bool = False - if self.parent_to_child_transform is None: + if parent_to_child_transform is not None: + self.parent_to_child_transform = parent_to_child_transform + + elif self.parent_to_child_transform is None: self.update_transform() if self.id is None: @@ -211,7 +214,7 @@ def add_fixed_constraint(self) -> None: """ Adds a fixed constraint between the parent link and the child link. """ - self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link) + self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, self.parent_to_child_transform) def calculate_transform(self) -> Transform: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 6ba34659d..11405ee4c 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -78,10 +78,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.description.update_description_from_file(self.path) if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - rdm = RobotDescriptionManager() - rdm.load_description(self.name) - World.robot = self - self._add_virtual_move_base_joints() + self._update_world_robot_and_description() self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name @@ -140,8 +137,20 @@ def _load_object_and_get_id(self, path: str, os.remove(path) raise e + def _update_world_robot_and_description(self): + """ + Initializes the robot description of the object, loads the description from the RobotDescriptionManager and sets + the robot as the current robot in the World. Also adds the virtual move base joints to the robot. + """ + rdm = RobotDescriptionManager() + rdm.load_description(self.name) + World.robot = self + self._add_virtual_move_base_joints() + def _add_virtual_move_base_joints(self): virtual_joints = RobotDescription.current_robot_description.virtual_move_base_joints + if virtual_joints is None: + return child_link = self.root_link_name axes = virtual_joints.get_axes() for joint_name, joint_type in virtual_joints.get_types().items(): @@ -387,7 +396,8 @@ def attach(self, child_object: Object, parent_link: Optional[str] = None, child_link: Optional[str] = None, - bidirectional: Optional[bool] = True) -> None: + bidirectional: Optional[bool] = True, + coincide_the_objects: Optional[bool] = False) -> None: """ Attaches another object to this object. This is done by saving the transformation between the given link, if there is one, and @@ -402,11 +412,16 @@ def attach(self, :param parent_link: The link name of this object. :param child_link: The link name of the other object. :param bidirectional: If the attachment should be a loose attachment. + :param coincide_the_objects: If True the object frames will be coincided. """ parent_link = self.links[parent_link] if parent_link else self.root_link child_link = child_object.links[child_link] if child_link else child_object.root_link - attachment = Attachment(parent_link, child_link, bidirectional) + if coincide_the_objects: + parent_to_child_transform = Transform() + else: + parent_to_child_transform = None + attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment child_object.attachments[self] = attachment.get_inverse() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a16a92712..9e0afd188 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -206,29 +206,10 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): return elif (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): - self.set_mobile_robot_pose(obj, pose) + self.set_mobile_robot_pose(pose) else: self._set_body_pose(obj.name, pose) - def set_mobile_robot_pose(self, robot: Object, pose: Pose): - """ - Set the pose of a mobile robot in the simulator by setting the position of the virtual move base joints. - param robot: The robot object. - param pose: The pose of the robot. - """ - current_position = robot.get_position() - position_diff = [pose.position.x - current_position.x, - pose.position.y - current_position.y] - current_angle = euler_from_quaternion(robot.get_orientation_as_list())[2] - angle_diff = euler_from_quaternion(pose.orientation_as_list())[2] - current_angle - # Get the joints of the base link - base_link_joints: VirtualMoveBaseJoints = RobotDescription.current_robot_description.virtual_move_base_joints - joint_name_position_dict = {base_link_joints.translation_x: position_diff[0], - base_link_joints.translation_y: position_diff[1], - base_link_joints.angular_z: angle_diff} - # get the pose in the joint frame - self.set_multiple_joint_positions(robot, joint_name_position_dict) - def multiverse_reset_world(self): self.writer.reset_world() diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 386581910..9a1265708 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -6,10 +6,11 @@ from typing_extensions import Optional, List from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint -from pycram.datastructures.enums import ObjectType +from pycram.datastructures.enums import ObjectType, Arms from pycram.datastructures.pose import Pose from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription +from pycram.robot_description import RobotDescription from pycram.world_concepts.world_object import Object multiverse_installed = True @@ -201,14 +202,15 @@ def test_detach_object(self): def test_attach_with_robot(self): milk = self.spawn_milk([1, 1, 0.1]) robot = self.spawn_robot() + ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector - milk_initial_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) - robot.attach(milk, robot.tip_link_name) + robot.attach(milk, ee_link.name, coincide_the_objects=True) self.assertTrue(robot in milk.attachments) - robot_position = robot.get_joint_position("joint1") + milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) + robot_position = robot.get_joint_position("arm_right_1_joint") robot_position += 1.57 - robot.set_joint_position("joint1", robot_position) - milk_pose = milk.root_link.get_pose_wrt_link(robot.tip_link) + robot.set_joint_position("arm_right_1_joint", robot_position) + milk_pose = milk.root_link.get_pose_wrt_link(ee_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): @@ -268,7 +270,7 @@ def spawn_milk(position: List, orientation: Optional[List] = None) -> Object: return milk def spawn_robot(self, position: Optional[List[float]] = None, - robot_name: Optional[str] = 'panda', + robot_name: Optional[str] = 'tiago_dual', replace: Optional[bool] = True) -> Object: if position is None: position = [-2, -2, 0.001] From 16609b4b41280f1559f0bc42e434994e8cfa38f5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 19 Jul 2024 17:18:14 +0200 Subject: [PATCH 058/189] [Multiverse] tests are running. --- src/pycram/description.py | 11 ++++++++++ .../world_concepts/multiverse_clients.py | 20 ++++++++++++++----- test/test_multiverse.py | 18 ++++++++--------- 3 files changed, 35 insertions(+), 14 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 1043ffac4..c19c11b3f 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -257,6 +257,17 @@ def is_root(self) -> bool: """ return self.object.get_root_link_id() == self.id + def set_pose(self, pose: Pose) -> None: + """ + Sets the pose of this link to the given pose. + NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints + that can allow the link to be at the given pose. + :param pose: The target pose for this link. + """ + self.object.set_pose( + (pose.to_transform(self.tf_frame) * self.get_transform_to_link(self.object.root_link)).to_pose() + ) + def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ Updates the transformation of this link at the given time. diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/world_concepts/multiverse_clients.py index 314c91901..6d7cfba54 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/world_concepts/multiverse_clients.py @@ -215,10 +215,14 @@ def join(self): class MultiverseWriter(MultiverseSocket): - time_for_sim_update: Optional[float] = 0.7 + time_for_sim_update: Optional[float] = 0.02 """ Wait time for the sent data to be applied in the simulator. """ + time_for_setting_body_data: Optional[float] = 0.01 + """ + Wait time for setting body data. + """ PORT: int = MultiverseReader.PORT + 1 """ @@ -292,7 +296,6 @@ def remove_body(self, body_name: str) -> None: self.send_data_to_server([time() - self.time_start], send_meta_data={body_name: []}, receive_meta_data={body_name: []}) - sleep(self.time_for_sim_update) def reset_world(self) -> None: """ @@ -322,7 +325,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_sim_update) + sleep(self.time_for_setting_body_data) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -354,6 +357,7 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() + sleep(self.time_for_sim_update) return self.response_meta_data @@ -377,6 +381,10 @@ class MultiverseAPI(MultiverseSocket): """ The port of the Multiverse API client for the prospection world. """ + API_REQUEST_WAIT_TIME: float = 0.2 + """ + The wait time for the API request in seconds. + """ def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): """ @@ -467,7 +475,7 @@ def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: param constraint: The constraint. return: The attachment pose as a string. """ - pose = constraint.child_link.get_pose_wrt_link(constraint.parent_link) + pose = constraint.parent_to_child_transform.to_pose() return self._pose_to_string(pose) @staticmethod @@ -629,7 +637,9 @@ def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: for api_name, params in api_data.items(): self._add_api_request(api_name, *params) self._send_api_request() - return self._get_all_apis_responses() + responses = self._get_all_apis_responses() + sleep(self.API_REQUEST_WAIT_TIME) + return responses def _get_all_apis_responses(self) -> APIDataDict: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9a1265708..5a16bd319 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -121,15 +121,15 @@ def test_update_position(self): def test_set_joint_position(self): if self.multiverse.robot is None: - robot = self.spawn_robot('tiago_dual') + robot = self.spawn_robot() else: robot = self.multiverse.robot step = 0.2 - for joint in ['torso_lift_joint', 'head_1_joint']: + for joint in ['torso_lift_joint']: original_joint_position = robot.get_joint_position(joint) - robot.set_joint_position(joint, original_joint_position - step) + robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - self.assertAlmostEqual(joint_position, original_joint_position - step, delta=0.01) + self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.01) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -158,7 +158,7 @@ def test_respawn_robot(self): # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): - self.spawn_robot(robot_name='tiago_dual') + self.spawn_robot() new_position = [-3, -3, 0.001] # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) self.multiverse.robot.set_position(new_position) @@ -207,15 +207,15 @@ def test_attach_with_robot(self): robot.attach(milk, ee_link.name, coincide_the_objects=True) self.assertTrue(robot in milk.attachments) milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) - robot_position = robot.get_joint_position("arm_right_1_joint") + robot_position = robot.get_joint_position("arm_right_2_joint") robot_position += 1.57 - robot.set_joint_position("arm_right_1_joint", robot_position) + robot.set_joint_position("arm_right_2_joint", robot_position) milk_pose = milk.root_link.get_pose_wrt_link(ee_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): for i in range(3): - milk = self.spawn_milk([1, 1, 0.1], [0, -0.707, 0, 0.707]) + milk = self.spawn_milk([1, 1, 0], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -232,7 +232,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.1], [0, -0.707, 0, 0.707]) - cup = self.spawn_cup([1, 1, 0.2]) + cup = self.spawn_cup([1, 1, 0.1]) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From 7332365abcf93fae095809bf0c5f713201a1875e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 19 Jul 2024 21:01:50 +0200 Subject: [PATCH 059/189] [Multiverse] restructure folders, and clients. Created client manager. --- src/pycram/datastructures/dataclasses.py | 8 +- src/pycram/datastructures/enums.py | 4 +- src/pycram/worlds/multiverse.py | 26 +- .../multiverse_communication/__init__.py | 0 .../client_manager.py | 72 +++++ .../multiverse_communication/clients.py} | 295 ++++++------------ .../multiverse_communication/socket.py} | 0 .../multiverse_datastructures/__init__.py | 0 .../multiverse_datastructures/dataclasses.py | 30 ++ .../worlds/multiverse_datastructures/enums.py | 13 + test/test_multiverse.py | 8 +- 11 files changed, 230 insertions(+), 226 deletions(-) create mode 100644 src/pycram/worlds/multiverse_communication/__init__.py create mode 100644 src/pycram/worlds/multiverse_communication/client_manager.py rename src/pycram/{world_concepts/multiverse_clients.py => worlds/multiverse_communication/clients.py} (67%) rename src/pycram/{world_concepts/multiverse_socket.py => worlds/multiverse_communication/socket.py} (100%) create mode 100644 src/pycram/worlds/multiverse_datastructures/__init__.py create mode 100644 src/pycram/worlds/multiverse_datastructures/dataclasses.py create mode 100644 src/pycram/worlds/multiverse_datastructures/enums.py diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 44e1ac0a8..f884f7127 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -2,7 +2,7 @@ from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from .enums import JointType, Shape, VirtualMoveBaseJointNames +from .enums import JointType, Shape, VirtualMoveBaseJointName from .pose import Pose, Point from abc import ABC, abstractmethod @@ -454,9 +454,9 @@ class VirtualMoveBaseJoints: """ Dataclass for storing the names, types and axes of the virtual move base joints of a mobile robot. """ - translation_x: Optional[str] = VirtualMoveBaseJointNames.LINEAR_X.value - translation_y: Optional[str] = VirtualMoveBaseJointNames.LINEAR_Y.value - angular_z: Optional[str] = VirtualMoveBaseJointNames.ANGULAR_Z.value + translation_x: Optional[str] = VirtualMoveBaseJointName.LINEAR_X.value + translation_y: Optional[str] = VirtualMoveBaseJointName.LINEAR_Y.value + angular_z: Optional[str] = VirtualMoveBaseJointName.ANGULAR_Z.value def get_types(self) -> Dict[str, JointType]: """ diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 03e321cb3..f1e95dba1 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -120,12 +120,10 @@ class GripperType(Enum): CUSTOM = auto() -class VirtualMoveBaseJointNames(Enum): +class VirtualMoveBaseJointName(Enum): """ Enum for the joint names of the virtual move base. """ LINEAR_X = "odom_vel_lin_x_joint" LINEAR_Y = "odom_vel_lin_y_joint" ANGULAR_Z = "odom_vel_ang_z_joint" - - diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 9e0afd188..cccb57320 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -2,18 +2,17 @@ import os import numpy as np -from tf.transformations import quaternion_matrix, euler_from_quaternion +from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, \ - VirtualMoveBaseJoints +from .multiverse_communication.client_manager import MultiverseClientManager +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint -from ..world_concepts.multiverse_clients import MultiverseReader, MultiverseWriter, MultiverseAPI from ..world_concepts.world_object import Object @@ -75,9 +74,11 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._spawn_floor() def _init_clients(self): - self.reader = MultiverseReader(is_prospection_world=self.is_prospection_world) - self.writer = MultiverseWriter(self.simulation, is_prospection_world=self.is_prospection_world) - self.api_requester = MultiverseAPI(self.simulation, is_prospection_world=self.is_prospection_world) + client_manager = MultiverseClientManager() + self.reader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) + self.writer = client_manager.create_writer(self.simulation, is_prospection_world=self.is_prospection_world) + self.api_requester = client_manager.create_api_requester(self.simulation, + is_prospection_world=self.is_prospection_world) def _set_world_job_flags(self): self.let_pycram_move_attached_objects = False @@ -150,7 +151,7 @@ def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: def get_object_joint_names(self, obj: Object) -> List[str]: return [joint.name for joint in obj.description.joints - if joint.type in self._joint_type_to_position_name.keys()] + if joint.type in self._joint_type_to_position_name.keys()] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] @@ -224,10 +225,7 @@ def _set_body_pose(self, body_name: str, pose: Pose) -> None: self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) def disconnect_from_physics_server(self) -> None: - self.writer.stop() - self.api_requester.stop() - self.reader.stop_thread = True - self.reader.join() + MultiverseClientManager.stop_all_clients() def join_threads(self) -> None: self.reader.stop_thread = True @@ -391,8 +389,8 @@ def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: logging.warning(f"Object {obj} does not exist in the simulator") - def check_object_exists_in_multiverse(self, object_name: str) -> bool: - return self.api_requester.check_object_exists(object_name) + def check_object_exists_in_multiverse(self, obj: Object) -> bool: + return self.api_requester.check_object_exists(obj) def get_resource_paths(dirname: str) -> List[str]: diff --git a/src/pycram/worlds/multiverse_communication/__init__.py b/src/pycram/worlds/multiverse_communication/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py new file mode 100644 index 000000000..42f8cffa2 --- /dev/null +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -0,0 +1,72 @@ +from typing_extensions import Optional, Type, Union, List, Dict + +from pycram.worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ + MultiverseReader + + +class MultiverseClientManager: + BASE_PORT: int = 9000 + """ + The base port of the Multiverse client. + """ + clients: Optional[Dict[str, MultiverseClient]] = {} + """ + The list of Multiverse clients. + """ + last_used_port: int = BASE_PORT + + def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': + """ + Create a Multiverse reader client. + param max_wait_time_for_data: The maximum wait time for the data in seconds. + param is_prospection_world: Whether the reader is connected to the prospection world. + """ + return self.create_client(MultiverseReader, "reader", is_prospection_world) + + def create_writer(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseWriter: + """ + Create a Multiverse writer client. + param simulation: The name of the simulation that the writer is connected to + (usually the name defined in the .muv file). + param is_prospection_world: Whether the writer is connected to the prospection world. + """ + return self.create_client(MultiverseWriter, "writer", is_prospection_world, + simulation=simulation) + + def create_api_requester(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseAPI: + """ + Create a Multiverse API client. + param simulation: The name of the simulation that the API is connected to + (usually the name defined in the .muv file). + param is_prospection_world: Whether the API is connected to the prospection world. + """ + return self.create_client(MultiverseAPI, "api_requester", is_prospection_world, simulation=simulation) + + def create_client(self, + client_type: Type[MultiverseClient], + name: Optional[str] = None, + is_prospection_world: Optional[bool] = False, + **kwargs) -> Union[MultiverseClient, MultiverseAPI, + MultiverseReader, MultiverseWriter]: + """ + Create a Multiverse client. + param kwargs: The keyword arguments. + """ + MultiverseClientManager.last_used_port += 1 + name = (name or client_type.__name__) + f"_{self.last_used_port}" + client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, **kwargs) + self.clients[name] = client + return client + + @classmethod + def stop_all_clients(cls): + """ + Stop all clients. + """ + for client in cls.clients: + if isinstance(client, MultiverseReader): + client.stop_thread = True + client.join() + elif isinstance(client, MultiverseClient): + client.stop() + cls.clients = {} diff --git a/src/pycram/world_concepts/multiverse_clients.py b/src/pycram/worlds/multiverse_communication/clients.py similarity index 67% rename from src/pycram/world_concepts/multiverse_clients.py rename to src/pycram/worlds/multiverse_communication/clients.py index 6d7cfba54..b668522ba 100644 --- a/src/pycram/world_concepts/multiverse_clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -1,96 +1,56 @@ import logging import threading -from dataclasses import dataclass from time import time, sleep from typing_extensions import List, Dict, Tuple, Optional -from .constraints import Constraint -from .multiverse_socket import MultiverseSocket, MultiverseMetaData, SocketAddress -from .world_object import Object, Link -from ..datastructures.world import World -from ..datastructures.pose import Pose +from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from ..multiverse_datastructures.enums import MultiverseAPIName as API +from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint +from ...datastructures.pose import Pose +from ...datastructures.world import World +from ...world_concepts.constraints import Constraint +from ...world_concepts.world_object import Object, Link -@dataclass -class RayResult: - """ - A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the - distance from the ray origin to the intersection point. - """ - body_name: str - distance: float +class MultiverseClient(MultiverseSocket): - def intersected(self) -> bool: + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): """ - Check if the ray intersects with a body. - return: Whether the ray intersects with a body. + Initialize the Multiverse client, which connects to the Multiverse server. + param name: The name of the client. + param port: The port of the client. + param is_prospection_world: Whether the client is connected to the prospection world. """ - return self.distance >= 0 and self.body_name != "" - - -@dataclass -class APIData: - """ - A dataclass to store the API data. - """ - api_name: str - params: List[str] - - @property - def as_dict(self) -> Dict: - return {self.api_name: self.params} - - -class APIDataDict(dict): - """ - A dictionary to store the API data, where the key is the API name and the value is the parameters. - """ + meta_data = MultiverseMetaData() + meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name + meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + super().__init__(SocketAddress(port=str(port)), meta_data) + self.run() - @classmethod - def from_list(cls, api_data_list: List[APIData]) -> "APIDataDict": - data = {api_data.api_name: api_data.params for api_data in api_data_list} - return cls(data) +class MultiverseReader(MultiverseClient): -@dataclass -class MultiverseContactPoint: + MAX_WAIT_TIME_FOR_DATA: Optional[float] = 1 """ - A dataclass to store the contact point returned from Multiverse. + The maximum wait time for the data in seconds. """ - body_name: str - contact_force: List[float] - contact_torque: List[float] - -class MultiverseReader(MultiverseSocket): - - PORT: int = 9000 - """ - The port of the Multiverse reader client. - """ - PROSPECTION_PORT: int = PORT + 3 - """ - The port of the Multiverse reader client for the prospection world.""" - - def __init__(self, max_wait_time_for_data: Optional[float] = 1, is_prospection_world: Optional[bool] = False): + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. This class provides methods to get data (e.g., position, orientation) from the Multiverse server. - param max_wait_time_for_data: The maximum wait time for the data in seconds. + param port: The port of the Multiverse reader client. param is_prospection_world: Whether the reader is connected to the prospection world. """ - self.max_wait_time_for_data = max_wait_time_for_data - meta_data = MultiverseMetaData() - meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "reader" - meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = self.PROSPECTION_PORT if is_prospection_world else self.PORT - super().__init__(SocketAddress(port=str(port)), meta_data) - self.run() + super().__init__(name, port, is_prospection_world) + + self.request_meta_data["receive"][""] = [""] + self.data_lock = threading.Lock() self.thread = threading.Thread(target=self.receive_all_data_from_server) self.stop_thread = False - self.request_meta_data["receive"][""] = [""] + self.thread.start() def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose]: @@ -168,7 +128,7 @@ def wait_for_body_data(self, name: str, properties: Optional[List[str]] = None) return: The body data as a dictionary. """ start = time() - while time() - start < self.max_wait_time_for_data: + while time() - start < self.MAX_WAIT_TIME_FOR_DATA: received_data = self.get_received_data() data_received_flag = self.check_for_body_data(name, received_data, properties) if data_received_flag: @@ -213,7 +173,7 @@ def join(self): self.thread.join() -class MultiverseWriter(MultiverseSocket): +class MultiverseWriter(MultiverseClient): time_for_sim_update: Optional[float] = 0.02 """ @@ -224,31 +184,18 @@ class MultiverseWriter(MultiverseSocket): Wait time for setting body data. """ - PORT: int = MultiverseReader.PORT + 1 - """ - The port of the Multiverse writer client. - """ - PROSPECTION_PORT: int = PORT + 3 - """ - The port of the Multiverse writer client for the prospection world. - """ - - def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. This class provides methods to send data (e.g., position, orientation) to the Multiverse server. + param port: The port of the Multiverse writer client. param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the writer is connected to the prospection world. """ - meta_data = MultiverseMetaData() - meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "writer" - meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = self.PROSPECTION_PORT if is_prospection_world else self.PORT - super().__init__(SocketAddress(port=str(port)), meta_data) + super().__init__(name, port, is_prospection_world) self.simulation = simulation self.time_start = time() - self.run() def _reset_request_meta_data(self): """ @@ -361,64 +308,47 @@ def send_data_to_server(self, data: List, return self.response_meta_data -class MultiverseAPI(MultiverseSocket): +class MultiverseAPI(MultiverseClient): - GET_CONTACT_BODIES_API_NAME = "get_contact_bodies" - GET_CONSTRAINT_EFFORT_API_NAME = "get_constraint_effort" - ATTACH_API_NAME = "attach" - DETACH_API_NAME = "detach" - GET_RAYS_API_NAME = "get_rays" - EXIST_API_NAME = "exist" + BASE_NAME: str = "api_requester" """ - The API names for the API callbacks to the Multiverse server. - """ - - PORT: int = MultiverseWriter.PORT + 1 - """ - The port of the Multiverse API client. - """ - PROSPECTION_PORT: int = PORT + 3 - """ - The port of the Multiverse API client for the prospection world. + The base name of the Multiverse reader. """ API_REQUEST_WAIT_TIME: float = 0.2 """ The wait time for the API request in seconds. """ - def __init__(self, simulation: str, is_prospection_world: Optional[bool] = False): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. This class provides methods like attach and detach objects, get contact points, and other API requests. + param port: The port of the Multiverse API client. param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the API is connected to the prospection world. """ - meta_data = MultiverseMetaData() - meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + "api_requester" - meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - port = self.PROSPECTION_PORT if is_prospection_world else self.PORT - super().__init__(SocketAddress(port=str(port)), meta_data) + super().__init__(name, port, is_prospection_world) self.simulation = simulation - self.run() def attach(self, constraint: Constraint) -> None: """ Request to attach the child link to the parent link. param constraint: The constraint. """ - self._request_single_api_callback(self._get_attach_api_data(constraint)) + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + attachment_pose = self._get_attachment_pose_as_string(constraint) + self._attach(child_link_name, parent_link_name, attachment_pose) - def _get_attach_api_data(self, constraint: Constraint) -> APIData: + def _attach(self, child_link_name: str, parent_link_name: str, attachment_pose: str) -> None: """ - Get the attach API data to be added to the api callback request metadata. - param constraint: The constraint. - return: The attach API data as an APIData. + Attach the child link to the parent link. + param child_link_name: The name of the child link. + param parent_link_name: The name of the parent link. + param attachment_pose: The attachment pose. """ - parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - return APIData(self.ATTACH_API_NAME, [child_link_name, - parent_link_name, - self._get_attachment_pose_as_string(constraint)]) + self._request_single_api_callback(API.ATTACH, child_link_name, parent_link_name, + attachment_pose) def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: """ @@ -458,16 +388,16 @@ def detach(self, constraint: Constraint) -> None: Request to detach the child link from the parent link. param constraint: The constraint. """ - self._request_single_api_callback(self._get_detach_api_data(constraint)) + parent_link_name, child_link_name = self.get_constraint_link_names(constraint) + self._detach(child_link_name, parent_link_name) - def _get_detach_api_data(self, constraint: Constraint) -> APIData: + def _detach(self, child_link_name: str, parent_link_name: str) -> None: """ - Get the detach API data to be added to the api callback request metadata. - param constraint: The constraint. - return: The detach API data as an APIData. + Detach the child link from the parent link. + param child_link_name: The name of the child link. + param parent_link_name: The name of the parent link. """ - parent_link_name, child_link_name = self.get_constraint_link_names(constraint) - return APIData(self.DETACH_API_NAME, [child_link_name, parent_link_name]) + self._request_single_api_callback(API.DETACH, child_link_name, parent_link_name) def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: """ @@ -488,22 +418,21 @@ def _pose_to_string(pose: Pose) -> str: return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ f"{pose.orientation.y} {pose.orientation.z}" - def check_object_exists(self, object_name: str) -> bool: + def check_object_exists(self, obj: Object) -> bool: """ Check if the object exists in the simulation. - param object_name: The name of the object. + param obj: The object. return: Whether the object exists in the simulation. """ - api_data = self._get_object_exists_api_data(object_name) - return self._request_single_api_callback(api_data)[0] == 'yes' + return self._object_exists(obj.name) - def _get_object_exists_api_data(self, object_name: str) -> APIData: + def _object_exists(self, object_name: str) -> bool: """ - Get the object exists API data to be added to the api callback request metadata. + Check if the object exists in the simulation. param object_name: The name of the object. - return: The object exists API data as an APIData. + return: Whether the object exists in the simulation. """ - return APIData(self.EXIST_API_NAME, [object_name]) + return self._request_single_api_callback(API.EXIST, object_name)[0] == 'yes' def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: """ @@ -511,34 +440,34 @@ def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: param obj: The object. return: The contact points of the object as a list of MultiverseContactPoint. """ - api_response_data = self._request_apis_callbacks(self._get_contact_points_api_data(obj)) - body_names = api_response_data[self.GET_CONTACT_BODIES_API_NAME] - contact_efforts = self._parse_constraint_effort(api_response_data[self.GET_CONSTRAINT_EFFORT_API_NAME]) + api_response_data = self._get_contact_points(obj.name) + body_names = api_response_data[API.GET_CONTACT_BODIES] + contact_efforts = self._parse_constraint_effort(api_response_data[API.GET_CONSTRAINT_EFFORT]) return [MultiverseContactPoint(body_names[i], contact_efforts[:3], contact_efforts[3:]) for i in range(len(body_names))] def get_objects_intersected_with_rays(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> List[RayResult]: """ - Get the rays from the from_positions to the to_positions. - param from_positions: The from positions of the rays. - param to_positions: The to positions of the rays. - return: The rays as a list of RayResult. + Get the rays intersections with the objects from the from_positions to the to_positions. + param from_positions: The starting positions of the rays. + param to_positions: The ending positions of the rays. + return: The rays intersections with the objects as a list of RayResult. """ - api_data = self._get_rays_api_data(from_positions, to_positions) - get_rays_response = self._request_single_api_callback(api_data) + get_rays_response = self._get_rays(from_positions, to_positions) return self._parse_get_rays_response(get_rays_response) - def _get_rays_api_data(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> APIData: + def _get_rays(self, from_positions: List[List[float]], + to_positions: List[List[float]]) -> List[str]: """ - Get the rays API data to be added to the api callback request metadata. - param from_positions: The from positions of the rays. - param to_positions: The to positions of the rays. - return: The rays API data as an APIData. + Get the rays intersections with the objects from the from_positions to the to_positions. + param from_positions: The starting positions of the rays. + param to_positions: The ending positions of the rays. + return: The rays intersections with the objects as a dictionary. """ - return APIData(self.GET_RAYS_API_NAME, [self.list_of_positions_to_string(from_positions), - self.list_of_positions_to_string(to_positions)] - ) + from_positions = self.list_of_positions_to_string(from_positions) + to_positions = self.list_of_positions_to_string(to_positions) + return self._request_single_api_callback(API.GET_RAYS, from_positions, to_positions) @staticmethod def _parse_get_rays_response(response: List[str]) -> List[RayResult]: @@ -575,59 +504,26 @@ def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: """ return list(map(float, contact_effort[0].split())) - def _request_objects_in_contact(self, obj: Object) -> List[str]: + def _get_contact_points(self, object_name) -> Dict[API, List]: """ - Request the objects in contact with an object. - param obj: The object. - return: The objects in contact as a list of strings for object names. - """ - return self._request_single_api_callback(self._get_contact_bodies_api_data(obj)) - - def _request_contact_effort(self, obj: Object) -> List[str]: - """ - Request the contact effort of an object. - param obj: The object. - return: The contact effort of the object as a list of strings that represent the contact forces and torques. - """ - return self._request_single_api_callback(self._get_constraint_effort_api_data(obj)) - - def _get_contact_points_api_data(self, obj: Object) -> APIDataDict: - """ - Get the contact points API data to be added to the api callback request metadata, this includes the - contact bodies and the contact effort api data. - param obj: The object. - return: The contact points API data as an APIDataDict. - """ - api_data_list = [self._get_contact_bodies_api_data(obj), self._get_constraint_effort_api_data(obj)] - return APIDataDict.from_list(api_data_list) - - @staticmethod - def _get_contact_bodies_api_data(obj: Object): - """ - Get the contact bodies API data to be added to the api callback request metadata. - param obj: The object. - """ - return APIData("get_contact_bodies", [obj.name]) - - @staticmethod - def _get_constraint_effort_api_data(obj: Object) -> APIData: - """ - Get the constraint effort API data to be added to the api callback request metadata. - param obj: The object. + Request the contact points of an object. + param object_name: The name of the object. + return: The contact points api response as a dictionary. """ - return APIData("get_constraint_effort", [obj.name]) + return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], + API.GET_CONSTRAINT_EFFORT: [object_name] + }) - def _request_single_api_callback(self, api_data: APIData) -> List[str]: + def _request_single_api_callback(self, api_name: API, *params) -> List[str]: """ Request a single API callback from the server. param api_data: The API data to request the callback. return: The API response as a list of strings. """ - api_data_dict = APIDataDict(api_data.as_dict) - response = self._request_apis_callbacks(api_data_dict) - return response[api_data.api_name] + response = self._request_apis_callbacks({api_name: list(params)}) + return response[api_name] - def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: + def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[str]]: """ Request the API callbacks from the server. param api_data_request: The API data to add to the request metadata. @@ -635,21 +531,20 @@ def _request_apis_callbacks(self, api_data: APIDataDict) -> APIDataDict: """ self._reset_api_callback() for api_name, params in api_data.items(): - self._add_api_request(api_name, *params) + self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() sleep(self.API_REQUEST_WAIT_TIME) return responses - def _get_all_apis_responses(self) -> APIDataDict: + def _get_all_apis_responses(self) -> Dict[API, List[str]]: """ Get all the API responses from the server. return: The API responses as a list of APIData. """ list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] - dict_of_api_responses = APIDataDict({api_name: response for api_response in list_of_api_responses - for api_name, response in api_response.items()}) - return dict_of_api_responses + return {API[api_name.upper()]: response for api_response in list_of_api_responses + for api_name, response in api_response.items()} def _add_api_request(self, api_name: str, *params): """ diff --git a/src/pycram/world_concepts/multiverse_socket.py b/src/pycram/worlds/multiverse_communication/socket.py similarity index 100% rename from src/pycram/world_concepts/multiverse_socket.py rename to src/pycram/worlds/multiverse_communication/socket.py diff --git a/src/pycram/worlds/multiverse_datastructures/__init__.py b/src/pycram/worlds/multiverse_datastructures/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_datastructures/dataclasses.py b/src/pycram/worlds/multiverse_datastructures/dataclasses.py new file mode 100644 index 000000000..914ea0628 --- /dev/null +++ b/src/pycram/worlds/multiverse_datastructures/dataclasses.py @@ -0,0 +1,30 @@ +from dataclasses import dataclass + +from typing_extensions import List + + +@dataclass +class RayResult: + """ + A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the + distance from the ray origin to the intersection point. + """ + body_name: str + distance: float + + def intersected(self) -> bool: + """ + Check if the ray intersects with a body. + return: Whether the ray intersects with a body. + """ + return self.distance >= 0 and self.body_name != "" + + +@dataclass +class MultiverseContactPoint: + """ + A dataclass to store the contact point returned from Multiverse. + """ + body_name: str + contact_force: List[float] + contact_torque: List[float] diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py new file mode 100644 index 000000000..7ffd8c8aa --- /dev/null +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -0,0 +1,13 @@ +from enum import Enum + + +class MultiverseAPIName(Enum): + """ + Enum for the different APIs of the Multiverse. + """ + GET_CONTACT_BODIES = "get_contact_bodies" + GET_CONSTRAINT_EFFORT = "get_constraint_effort" + ATTACH = "attach" + DETACH = "detach" + GET_RAYS = "get_rays" + EXIST = "exist" diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5a16bd319..030208b1f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import time import unittest import psutil @@ -10,13 +9,12 @@ from pycram.datastructures.pose import Pose from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription -from pycram.robot_description import RobotDescription from pycram.world_concepts.world_object import Object multiverse_installed = True try: from pycram.worlds.multiverse import Multiverse - from pycram.world_concepts.multiverse_socket import SocketAddress + from pycram.worlds.multiverse_communication.socket import SocketAddress except ImportError: multiverse_installed = False @@ -99,11 +97,11 @@ def test_remove_object(self): milk = self.spawn_milk([1, 1, 0.1]) milk.remove() self.assertTrue(milk not in self.multiverse.objects) - self.assertFalse(self.multiverse.check_object_exists_in_multiverse(milk.name)) + self.assertFalse(self.multiverse.check_object_exists_in_multiverse(milk)) def test_check_object_exists(self): milk = self.spawn_milk([1, 1, 0.1]) - self.assertTrue(self.multiverse.check_object_exists_in_multiverse(milk.name)) + self.assertTrue(self.multiverse.check_object_exists_in_multiverse(milk)) def test_set_position(self): milk = self.spawn_milk([1, 1, 0.1]) From 18d6b8ed6780c0cafd8ed3fb3d8cf34c89a125bb Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 21 Jul 2024 23:38:22 +0200 Subject: [PATCH 060/189] [Multiverse] Implementing goal validation for multiverse. --- src/pycram/datastructures/world.py | 15 +- src/pycram/description.py | 72 +++++-- src/pycram/object_descriptors/urdf.py | 9 +- src/pycram/world_concepts/constraints.py | 43 +++- src/pycram/world_concepts/world_object.py | 58 ++++- src/pycram/worlds/bullet_world.py | 6 +- src/pycram/worlds/multiverse.py | 186 ++++++++++++---- .../client_manager.py | 10 +- .../multiverse_communication/clients.py | 153 ++++++++++--- .../worlds/multiverse_functions/__init__.py | 0 .../multiverse_functions/goal_validator.py | 201 ++++++++++++++++++ test/test_multiverse.py | 6 +- 12 files changed, 641 insertions(+), 118 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/__init__.py create mode 100644 src/pycram/worlds/multiverse_functions/goal_validator.py diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ec061d7b3..e2dbbdf23 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -137,7 +137,7 @@ class World(StateEntity, ABC): the objects. """ - data_directory: List[str] = [resources_path, os.path.join(resources_path, 'robots')] + data_directory: List[str] = [resources_path] """ Global reference for the data directories, this is used to search for the description files of the robot, the objects, and the cached files. @@ -498,7 +498,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: goal = self.get_move_base_joint_goal(pose) self.robot.set_joint_positions(goal) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. param pose: The target pose. @@ -508,9 +508,9 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() - return {move_base_joints.translation_x: position_diff[0], - move_base_joints.translation_y: position_diff[1], - move_base_joints.angular_z: angle_diff} + return {self.robot.joints[move_base_joints.translation_x]: position_diff[0], + self.robot.joints[move_base_joints.translation_y]: position_diff[1], + self.robot.joints[move_base_joints.angular_z]: angle_diff} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: @@ -1196,11 +1196,10 @@ def __del__(self): self.exit() @abstractmethod - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: """ Set the positions of multiple joints of an articulated object. - :param obj: The object. - :param joint_poses: A dictionary with joint names as keys and joint positions as values. + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. """ pass diff --git a/src/pycram/description.py b/src/pycram/description.py index c19c11b3f..83f796ac2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -62,8 +62,9 @@ class JointDescription(EntityDescription): A class that represents the description of a joint. """ - def __init__(self, parsed_joint_description: Optional[Any] = None): + def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: bool = False): self.parsed_description = parsed_joint_description + self.is_virtual: Optional[bool] = is_virtual @property @abstractmethod @@ -158,6 +159,13 @@ def __init__(self, _id: int, obj: Object): WorldEntity.__init__(self, _id, obj.world) self.object: Object = obj + @property + def object_name(self) -> str: + """ + The name of the object to which this joint belongs. + """ + return self.object.name + @property @abstractmethod def pose(self) -> Pose: @@ -205,6 +213,27 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self.constraint_ids: Dict[Link, int] = {} self._update_pose() + def set_pose(self, pose: Pose) -> None: + """ + Sets the pose of this link to the given pose. + NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints + that can allow the link to be at the given pose. + :param pose: The target pose for this link. + """ + self.object.set_pose(self.get_object_pose_given_link_pose(pose)) + + def get_object_pose_given_link_pose(self, pose): + return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() + + def get_pose_given_object_pose(self, pose): + return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() + + def get_transform_from_root_link(self) -> Transform: + return self.get_transform_from_link(self.object.root_link) + + def get_transform_to_root_link(self) -> Transform: + return self.get_transform_to_link(self.object.root_link) + @property def current_state(self) -> LinkState: return LinkState(self.constraint_ids.copy()) @@ -213,17 +242,18 @@ def current_state(self) -> LinkState: def current_state(self, link_state: LinkState) -> None: self.constraint_ids = link_state.constraint_ids - def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional[Transform] = None) -> int: + def add_fixed_constraint_with_link(self, child_link: 'Link', + child_to_parent_transform: Optional[Transform] = None) -> int: """ Adds a fixed constraint between this link and the given link, used to create attachments for example. :param child_link: The child link to which a fixed constraint should be added. - :param transform: The transformation between the two links. + :param child_to_parent_transform: The transformation between the two links. :return: The unique id of the constraint. """ - if transform is None: - transform = child_link.get_transform_from_link(self) - constraint_id = self.world.add_fixed_constraint(self, child_link, transform) + if child_to_parent_transform is None: + child_to_parent_transform = child_link.get_transform_to_link(self) + constraint_id = self.world.add_fixed_constraint(self, child_link, child_to_parent_transform) self.constraint_ids[child_link] = constraint_id child_link.constraint_ids[self] = constraint_id return constraint_id @@ -257,17 +287,6 @@ def is_root(self) -> bool: """ return self.object.get_root_link_id() == self.id - def set_pose(self, pose: Pose) -> None: - """ - Sets the pose of this link to the given pose. - NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints - that can allow the link to be at the given pose. - :param pose: The target pose for this link. - """ - self.object.set_pose( - (pose.to_transform(self.tf_frame) * self.get_transform_to_link(self.object.root_link)).to_pose() - ) - def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ Updates the transformation of this link at the given time. @@ -451,9 +470,9 @@ class Joint(ObjectEntity, JointDescription, ABC): def __init__(self, _id: int, joint_description: JointDescription, - obj: Object): + obj: Object, is_virtual: Optional[bool] = False): ObjectEntity.__init__(self, _id, obj) - JointDescription.__init__(self, joint_description.parsed_description) + JointDescription.__init__(self, joint_description.parsed_description, is_virtual) self._update_position() @property @@ -598,10 +617,22 @@ def __init__(self, path: Optional[str] = None): else: self._parsed_description = None + self.virtual_joint_names: List[str] = [] + + def is_joint_virtual(self, name: str) -> bool: + """ + Returns whether the joint with the given name is virtual. + + :param name: The name of the joint. + :return: True if the joint is virtual, False otherwise. + """ + return name in self.virtual_joint_names + @abstractmethod def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, - lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: """ Adds a joint to this object. @@ -613,6 +644,7 @@ def add_joint(self, name: str, child: str, joint_type: JointType, :param origin: The origin of the joint. :param lower_limit: The lower limit of the joint. :param upper_limit: The upper limit of the joint. + :param is_virtual: True if the joint is virtual, False otherwise. """ pass diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 7288df776..c5b51fb60 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -81,8 +81,8 @@ class JointDescription(AbstractJointDescription): pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} - def __init__(self, urdf_description: urdf.Joint): - super().__init__(urdf_description) + def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False): + super().__init__(urdf_description, is_virtual=is_virtual) @property def origin(self) -> Pose: @@ -176,7 +176,8 @@ class Joint(AbstractObjectDescription.Joint, JointDescription): def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, - lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: if lower_limit is not None or upper_limit is not None: limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) else: @@ -195,6 +196,8 @@ def add_joint(self, name: str, child: str, joint_type: JointType, JointDescription.pycram_type_map[joint_type], axis, origin, limit) self.parsed_description.add_joint(joint) + if is_virtual: + self.virtual_joint_names.append(name) def load_description(self, path) -> URDF: with open(path, 'r') as file: diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index ff4514e0d..955336925 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -30,6 +30,38 @@ def __init__(self, self.child_to_constraint = child_to_constraint self._parent_to_child = None + def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: + """ + Returns the pose of the child object given the parent pose. + + :param pose: The parent object pose. + :return: The pose of the child object. + """ + pose = self.parent_link.get_pose_given_object_pose(pose) + child_link_pose = self.get_child_link_target_pose_given_parent(pose) + return self.child_link.get_object_pose_given_link_pose(child_link_pose) + + def set_child_link_pose(self): + """ + Sets the target pose of the child object to the current pose of the child object in the parent object frame. + """ + self.child_link.set_pose(self.get_child_link_target_pose()) + + def get_child_link_target_pose(self) -> Pose: + """ + Returns the target pose of the child object. (The pose of the child object in the parent object frame) + """ + return self.parent_to_child_transform.to_pose() + + def get_child_link_target_pose_given_parent(self, parent_pose: Pose) -> Pose: + """ + Returns the target pose of the child object link given the parent link pose. + + :param parent_pose: The parent link pose. + :return: The target pose of the child object link. + """ + return (parent_pose.to_transform(self.parent_link.tf_frame) * self.parent_to_child_transform).to_pose() + @property def parent_to_child_transform(self) -> Union[Transform, None]: if self._parent_to_child is None: @@ -190,6 +222,14 @@ def __init__(self, if self.id is None: self.add_fixed_constraint() + @property + def parent_object(self): + return self.parent_link.object + + @property + def child_object(self): + return self.child_link.object + def update_transform_and_constraint(self) -> None: """ Updates the transform and constraint of this attachment. @@ -214,7 +254,8 @@ def add_fixed_constraint(self) -> None: """ Adds a fixed constraint between the parent link and the child link. """ - self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, self.parent_to_child_transform) + self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, + self.parent_to_child_transform.invert()) def calculate_transform(self) -> Transform: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 11405ee4c..f64baa339 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -92,6 +92,31 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + + def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: + """ + Get the target poses of the attached objects. + return: The target poses of the attached objects + """ + return self.get_target_poses_of_attached_objects_given_parent(self.get_pose()) + + def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: + """ + Get the poses of the attached objects. + return: The poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose() + for child_object, attachment in self.attachments.items() if not attachment.loose} + + def get_target_poses_of_attached_objects_given_parent(self, pose: Pose) -> Dict[Object, Pose]: + """ + Get the target poses of the attached objects of an object. Given the pose of the parent object. + param pose: The pose of the parent object. + return: The target poses of the attached objects + """ + return {child_object: attachment.get_child_object_pose_given_parent(pose) for child_object, attachment + in self.attachments.items() if not attachment.loose} + @property def pose(self): return self.get_pose() @@ -195,8 +220,20 @@ def _init_joints(self): """ self.joints = {} for joint_name, joint_id in self.joint_name_to_id.items(): - joint_description = self.description.get_joint_by_name(joint_name) - self.joints[joint_name] = self.description.Joint(joint_id, joint_description, self) + parsed_joint_description = self.description.get_joint_by_name(joint_name) + is_virtual = self.is_joint_virtual(joint_name) + self.joints[joint_name] = self.description.Joint(joint_id, parsed_joint_description, self, is_virtual) + + def is_joint_virtual(self, name: str): + return self.description.is_joint_virtual(name) + + @property + def virtual_joint_names(self): + return self.description.virtual_joint_names + + @property + def virtual_joints(self): + return [joint for joint in self.joints.values() if joint.is_virtual] @property def has_one_link(self) -> bool: @@ -420,7 +457,7 @@ def attach(self, if coincide_the_objects: parent_to_child_transform = Transform() else: - parent_to_child_transform = None + parent_to_child_transform = parent_link.get_transform_to_link(child_link) attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment @@ -694,8 +731,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec child.update_attachment_with_object(self) else: - link_to_object = attachment.parent_to_child_transform - child.set_pose(link_to_object.to_pose(), set_attachments=False) + child.set_pose(attachment.get_child_link_target_pose(), set_attachments=False) child._set_attached_objects_poses(already_moved_objects + [self]) def set_position(self, position: Union[Pose, Point, List], base=False) -> None: @@ -833,21 +869,23 @@ def reset_all_joints_positions(self) -> None: """ Sets the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = list(self.joint_name_to_id.keys()) + joint_names = [joint.name for joint in self.joints.values() if not joint.is_virtual] if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_poses: dict) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. - :param joint_poses: + :param joint_positions: A dictionary with the joint names as keys and the target positions as values. """ - self.world.set_multiple_joint_positions(self, joint_poses) - # self.update_pose() + joint_positions = {self.joints[joint_name]: joint_position + for joint_name, joint_position in joint_positions.items()} + self.world.set_multiple_joint_positions(joint_positions) + self.update_pose() self._update_all_links_poses() self.update_link_transforms() self._set_attached_objects_poses() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 7af5ca325..f18753979 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -167,9 +167,9 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: - for joint_name, joint_position in joint_poses.items(): - self.reset_joint_position(obj.joints[joint_name], joint_position) + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + for joint, joint_position in joint_positions.items(): + self.reset_joint_position(joint, joint_position) def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index cccb57320..7971a0850 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,11 +1,14 @@ import logging import os +from time import sleep, time import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional +from typing_extensions import List, Dict, Optional, Iterable, Tuple from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ + MultiPoseGoalValidator, JointGoalValidator, MultiJointGoalValidator from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -40,6 +43,28 @@ class Multiverse(World): the multiverse configuration file). """ + try: + simulation_wait_time_factor = float(os.environ['Multiverse_Simulation_Wait_Time_Factor']) + except KeyError: + simulation_wait_time_factor = 1.0 + """ + The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for + the time taken by the simulation to process the request, this depends on the computational power of the machine + running the simulation. + TODO: This should be replaced by a feedback mechanism that waits until a certain condition is met, e.g. the action + is completed. + """ + + position_tol: float = 2e-3 + """ + The tolerance for position comparison. (e.g. for checking if the object has reached the desired position) + """ + + orientation_tol: float = 2e-3 + """ + The tolerance for orientation comparison. (e.g. for checking if the object has reached the desired orientation) + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, @@ -50,6 +75,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, param is_prospection: Whether the world is prospection or not. param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. + param simulation: The name of the simulation. """ self._make_sure_multiverse_resources_are_added() @@ -74,7 +100,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._spawn_floor() def _init_clients(self): - client_manager = MultiverseClientManager() + client_manager = MultiverseClientManager(self.simulation_wait_time_factor) self.reader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) self.writer = client_manager.create_writer(self.simulation, is_prospection_world=self.is_prospection_world) self.api_requester = client_manager.create_api_requester(self.simulation, @@ -102,8 +128,7 @@ def _make_sure_multiverse_resources_are_added(self): if not self.added_multiverse_resources: World.cache_manager.clear_cache() dirname = find_multiverse_resources_path() - resources_paths = get_resource_paths(dirname) - World.data_directory = resources_paths + self.data_directory + World.data_directory = [dirname] + self.data_directory World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True @@ -156,32 +181,49 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] - def get_joint_position(self, joint: Joint) -> float: - self.check_object_exists_and_issue_warning_if_not(joint.object) - property_name = self.get_joint_position_name(joint) - return self.get_body_property(joint.name, property_name) - - def get_body_property(self, body_name: str, property_name: str) -> float: - return self.reader.get_body_property(body_name, property_name)[0] - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - attribute = self.get_joint_position_name(joint) - self.send_body_data_to_server(joint.name, {attribute: [joint_position]}) - - def set_multiple_joint_positions(self, obj: Object, joint_poses: Dict[str, float]) -> None: - data = {joint.name: {self.get_joint_position_name(joint): [joint_poses[joint.name]]} - for joint in obj.joints.values() if joint.name in joint_poses.keys()} - if len(data) > 0: - self.writer.send_multiple_body_data_to_server(data) - else: - logging.warning(f"No joints found in object {obj.name}") - raise ValueError(f"No joints found in object {obj.name}") - - def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]): - self.writer.send_body_data_to_server(body_name, data) + self.set_multiple_joint_positions({joint: joint_position}) + + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + initial_joint_positions = {joint: self.get_joint_position(joint) for joint in joint_positions.keys()} + data = {joint.name: {self.get_joint_position_name(joint): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(data) + self._wait_until_multiple_joint_goals_are_achieved(joint_positions, initial_joint_positions) + + def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Joint, float], + initial_joint_positions: Dict[Joint, float]) -> None: + goal_validator = self._get_multi_joint_goal_validator(joint_positions, initial_joint_positions) + self._wait_until_goal_is_achieved(goal_validator) + + def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], + initial_joint_positions: Dict[Joint, float]) -> MultiJointGoalValidator: + joints = list(joint_positions.keys()) + target_joint_positions = list(joint_positions.values()) + initial_joint_positions = list(initial_joint_positions.values()) + goal_validator = MultiJointGoalValidator(initial_joint_positions, target_joint_positions, + lambda: list(self.get_multiple_joint_positions(joints).values()), + joints[0].object_name + "_joint_goal") + return goal_validator - def sent_data_to_server(self, data: Dict[str, List[float]]): - self.writer.send_data_to_server(list(data.values())) + def get_joint_position(self, joint: Joint) -> float: + data = self.get_multiple_joint_positions([joint]) + if data is not None: + return data[joint.name] + + def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: + self.check_object_exists_and_issue_warning_if_not(joints[0].object) + joint_names = [joint.name for joint in joints] + data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] + for joint in joints}) + if data is not None: + return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} + + def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, + initial_joint_position: float) -> None: + goal_validator = JointGoalValidator(initial_joint_position, joint_position, + lambda: self.get_joint_position(joint), joint.name + "_joint_goal") + self._wait_until_goal_is_achieved(goal_validator) def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) @@ -193,23 +235,56 @@ def get_object_pose(self, obj: Object) -> Pose: return Pose() return self._get_body_pose(obj.name) - def _get_body_pose(self, body_name: str) -> Pose: - """ - Get the pose of a body in the simulator. - param body_name: The name of the body. - return: The pose of the body. - """ - return self.reader.get_body_pose(body_name) - def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return - elif (obj.obj_type == ObjectType.ROBOT and - RobotDescription.current_robot_description.virtual_move_base_joints is not None): + if (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_move_base_joints is not None): self.set_mobile_robot_pose(pose) else: + initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) self._set_body_pose(obj.name, pose) + if len(initial_attached_objects_poses) > 0: + self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: + """ + Wait until all attached objects are set to the desired poses. + param obj: The object to which the attached objects belong. + param initial_poses: The list of initial poses of the attached objects. + """ + target_poses = obj.get_target_poses_of_attached_objects() + self._wait_until_all_pose_goals_are_achieved(list(target_poses.keys()), list(target_poses.values()), + initial_poses) + + def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], + initial_poses: List[Pose], + name: Optional[str] = "all_poses_goal") -> None: + """ + Wait until all poses are set to the desired poses. + param poses: The dictionary of the desired poses + param initial_poses: The dictionary of the initial poses + param name: The name of the goal. + """ + goal_validator = MultiPoseGoalValidator(initial_poses, poses, + lambda: list(self._get_multiple_body_poses(body_names).values()), name) + self._wait_until_goal_is_achieved(goal_validator) + + def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: + """ + Get the pose of a body in the simulator. + param body_name: The name of the body. + :param wait: Whether to wait until the pose is received. + return: The pose of the body. + """ + return self.reader.get_body_pose(body_name, wait) + + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([obj.name for obj in objects]) + + def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: + return self.reader.get_multiple_body_poses(body_names) def multiverse_reset_world(self): self.writer.reset_world() @@ -222,7 +297,39 @@ def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ xyzw = pose.orientation_as_list() wxyz = [xyzw[3], *xyzw[:3]] + initial_pose = self._get_body_pose(body_name, wait=False) self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) + if initial_pose is not None: + self._wait_until_pose_goal_is_achieved(body_name, pose, initial_pose) + + def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, initial_pose: Pose): + """ + Wait until the pose of a body is set. + param body_name: The name of the body. + param target_pose: The target pose of the body. + param initial_pose: The initial pose of the body. + """ + goal_validator = PoseGoalValidator(initial_pose, target_pose, lambda: self._get_body_pose(body_name), + body_name + "_pose_goal") + self._wait_until_goal_is_achieved(goal_validator) + + def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: + """ + Wait until the target is reached. + param goal_validator: The goal validator object to validate and keep track of the goal achievement progress. + """ + start_time = time() + current = goal_validator.current_value + while not goal_validator.goal_achieved: + sleep(0.01) + if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: + msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to"\ + f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}"\ + f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage"\ + f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" + logging.error(msg) + raise TimeoutError(msg) + current = goal_validator.current_value def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() @@ -387,7 +494,8 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_and_issue_warning_if_not(self, obj: Object): if obj not in self.objects: - logging.warning(f"Object {obj} does not exist in the simulator") + msg = f"Object {obj} does not exist in the simulator" + logging.warning(msg) def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 42f8cffa2..750213ca7 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -15,6 +15,13 @@ class MultiverseClientManager: """ last_used_port: int = BASE_PORT + def __init__(self, simulation_wait_time_factor: Optional[float] = 1.0): + """ + Initialize the Multiverse client manager. + param simulation_wait_time_factor: The simulation wait time factor. + """ + self.simulation_wait_time_factor = simulation_wait_time_factor + def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': """ Create a Multiverse reader client. @@ -54,7 +61,8 @@ def create_client(self, """ MultiverseClientManager.last_used_port += 1 name = (name or client_type.__name__) + f"_{self.last_used_port}" - client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, **kwargs) + client = client_type(name, self.last_used_port, is_prospection_world=is_prospection_world, + simulation_wait_time_factor=self.simulation_wait_time_factor, **kwargs) self.clients[name] = client return client diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index b668522ba..0feb5e1ff 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -15,35 +15,41 @@ class MultiverseClient(MultiverseSocket): - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse client, which connects to the Multiverse server. param name: The name of the client. param port: The port of the client. param is_prospection_world: Whether the client is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor (default is 1.0), which can be used to + increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name super().__init__(SocketAddress(port=str(port)), meta_data) + self.simulation_wait_time_factor = simulation_wait_time_factor self.run() class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: Optional[float] = 1 + MAX_WAIT_TIME_FOR_DATA: Optional[float] = 2 """ The maximum wait time for the data in seconds. """ - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. This class provides methods to get data (e.g., position, orientation) from the Multiverse server. param port: The port of the Multiverse reader client. param is_prospection_world: Whether the reader is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.request_meta_data["receive"][""] = [""] @@ -60,9 +66,22 @@ def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose param wait: Whether to wait for the data. return: The position and orientation of the body. """ - data = self.get_body_data(name, ["position", "quaternion"], wait=wait) + data = self.get_multiple_body_poses([name], wait=wait) if data is not None: - return Pose(data["position"], self.wxyz_to_xyzw(data["quaternion"])) + return data[name] + + def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = True) -> Optional[Dict[str, Pose]]: + """ + Get the body poses from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The positions and orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_data(body_names, {name: ["position", "quaternion"] for name in body_names}, + wait=wait) + if data is not None: + return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) + for name in body_names} def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -71,7 +90,17 @@ def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[ param wait: Whether to wait for the data. return: The position of the body. """ - return self.get_body_data(name, ["position"], wait=wait) + return self.get_multiple_body_positions([name], wait=wait)[name] + + def get_multiple_body_positions(self, body_names: List[str], + wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + """ + Get the body positions from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The positions of the bodies as a dictionary. + """ + return self.get_multiple_body_properties(body_names, ["position"], wait=wait) def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -80,9 +109,19 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Option param wait: Whether to wait for the data. return: The orientation of the body. """ - data = self.get_body_property(name, "quaternion", wait=wait) + return self.get_multiple_body_orientations([name], wait=wait)[name] + + def get_multiple_body_orientations(self, body_names: List[str], + wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + """ + Get the body orientations from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param wait: Whether to wait for the data. + return: The orientations of the bodies as a dictionary. + """ + data = self.get_multiple_body_properties(body_names, ["quaternion"], wait=wait) if data is not None: - return self.wxyz_to_xyzw(data) + return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} def get_body_property(self, name: str, property_name: str, wait: Optional[bool] = True) -> Optional[List[float]]: """ @@ -92,9 +131,20 @@ def get_body_property(self, name: str, property_name: str, wait: Optional[bool] param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_body_data(name, [property_name], wait=wait) + data = self.get_multiple_body_properties([name], [property_name], wait=wait) if data is not None: - return data[property_name] + return data[name][property_name] + + def get_multiple_body_properties(self, body_names: List[str], properties: List[str], + wait: Optional[bool] = True) -> Dict[str, Dict[str, List[float]]]: + """ + Get the body properties from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param wait: Whether to wait for the data. + return: The properties of the bodies as a dictionary. + """ + return self.get_multiple_body_data(body_names, {name: properties for name in body_names}, wait=wait) @staticmethod def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: @@ -115,24 +165,60 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ + properties = {name: properties} if properties is not None else None + return self.get_multiple_body_data([name], properties, wait=wait)[name] + + def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, + wait: Optional[bool] = True) -> Dict: + """ + Get the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param wait: Whether to wait for the data. + return: The body data as a dictionary. + """ + if wait: - return self.wait_for_body_data(name, properties) - elif self.check_for_body_data(name, self.get_received_data(), properties): - return self.get_received_data()[name] + return self.wait_for_multiple_body_data(body_names, properties) + + data = self.get_received_data() + if self.check_multiple_body_data(body_names, data, properties): + return {name: data[name] for name in body_names} - def wait_for_body_data(self, name: str, properties: Optional[List[str]] = None) -> Dict: + def wait_for_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[str]]] = None) -> Dict: """ - Wait for the body data from the multiverse server. - param name: The name of the body. - param properties: The properties of the body. + Wait for the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. return: The body data as a dictionary. """ start = time() + data_received_flag = False while time() - start < self.MAX_WAIT_TIME_FOR_DATA: received_data = self.get_received_data() - data_received_flag = self.check_for_body_data(name, received_data, properties) + data_received_flag = self.check_multiple_body_data(body_names, received_data, properties) if data_received_flag: - return received_data[name] + return {name: received_data[name] for name in body_names} + if not data_received_flag: + properties_str = "Data" if properties is None else f"Properties {properties}" + msg = f"{properties_str} for bodies {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" + logging.error(msg) + raise ValueError(msg) + + def check_multiple_body_data(self, body_names: List[str], data: Dict, + properties: Optional[Dict[str, List[str]]] = None) -> bool: + """ + Check if the body data is received from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param data: The data received from the multiverse server. + param properties: The properties of the bodies. + return: Whether the body data is received. + """ + if properties is None: + return all([self.check_for_body_data(name, data) for name in body_names]) + else: + return all([self.check_for_body_data(name, data, properties[name]) for name in body_names]) @staticmethod def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = None) -> bool: @@ -175,16 +261,17 @@ def join(self): class MultiverseWriter(MultiverseClient): - time_for_sim_update: Optional[float] = 0.02 + time_for_sim_update: Optional[float] = 0.0 # 0.02 """ Wait time for the sent data to be applied in the simulator. """ - time_for_setting_body_data: Optional[float] = 0.01 + time_for_setting_body_data: Optional[float] = 0.0 # 0.01 """ Wait time for setting body data. """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. This class provides methods to send data (e.g., position, orientation) to the Multiverse server. @@ -192,8 +279,10 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the writer is connected to the prospection world. + param simulation_wait_time_factor: The wait time factor for the simulation (default is 1.0), which can be used + to increase or decrease the wait time for the simulation. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation self.time_start = time() @@ -217,7 +306,8 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List """ self.send_body_data_to_server(body_name, {"position": position, - "quaternion": orientation}) + "quaternion": orientation, + "relative_velocity": [0]*6}) def set_body_position(self, body_name: str, position: List[float]) -> None: """ @@ -272,7 +362,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_setting_body_data) + sleep(self.time_for_setting_body_data * self.simulation_wait_time_factor) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -304,7 +394,7 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() - sleep(self.time_for_sim_update) + sleep(self.time_for_sim_update * self.simulation_wait_time_factor) return self.response_meta_data @@ -319,7 +409,8 @@ class MultiverseAPI(MultiverseClient): The wait time for the API request in seconds. """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False): + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + simulation_wait_time_factor: Optional[float] = 1.0): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. This class provides methods like attach and detach objects, get contact points, and other API requests. @@ -327,8 +418,10 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). param is_prospection_world: Whether the API is connected to the prospection world. + param simulation_wait_time_factor: The simulation wait time factor, which can be used to increase or decrease + the wait time for the simulation. """ - super().__init__(name, port, is_prospection_world) + super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation def attach(self, constraint: Constraint) -> None: @@ -534,7 +627,7 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() - sleep(self.API_REQUEST_WAIT_TIME) + sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) return responses def _get_all_apis_responses(self) -> Dict[API, List[str]]: diff --git a/src/pycram/worlds/multiverse_functions/__init__.py b/src/pycram/worlds/multiverse_functions/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py new file mode 100644 index 000000000..423f5c5e5 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -0,0 +1,201 @@ +from abc import abstractmethod, ABC + +import numpy as np +from typing_extensions import List, Union, Any, Callable + +from pycram.datastructures.pose import Pose + + +class GoalValidator(ABC): + """ + A class to validate the goal by tracking the goal achievement progress. + """ + + def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + self.initial_value: Any = initial_value + self.current_value_getter: Callable[[], Any] = current_value_getter + self.goal_value: Any = goal_value + self.name: str = name + self.acceptable_percentage_of_goal_achieved: float = acceptable_percentage_of_goal_achieved + self.initial_error: float = self.calculate_initial_error() + + def calculate_initial_error(self) -> float: + """ + Calculate the initial error. + """ + return self.calculate_error(self.goal_value, self.initial_value) + + @property + def current_value(self) -> Any: + """ + Get the current value. + """ + return self.current_value_getter() + + @property + def current_error(self) -> float: + """ + Calculate the current error. + """ + return self.calculate_error(self.goal_value, self.current_value) + + @abstractmethod + def calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two values. + """ + pass + + @property + def percentage_of_goal_achieved(self) -> float: + """ + Calculate the percentage of goal achieved. + """ + if self.initial_error > 1e-3: + return 1 - self.current_error / self.initial_error + else: + return 1 + + @property + def goal_achieved(self) -> bool: + """ + Check if the goal is achieved. + return: Whether the goal is achieved. + """ + return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved + + +class SingleValueGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for a single value. + """ + + def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], + current_value_getter: Callable[[], Union[float, int]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: + """ + Calculate the error between two values. + return: The error between the two values. + """ + return abs(value_1 - value_2) + + +class JointGoalValidator(SingleValueGoalValidator): + def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], + current_value_getter: Callable[[], Union[float, int]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class IterableGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for an iterable goal. + """ + + def __init__(self, initial_value: List[Any], goal_value: List[Any], + current_value_getter: Callable[[], List[Any]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return np.linalg.norm(np.array(iterable_1) - np.array(iterable_2)) + + +def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_name: str): + """ + Get a combined goal validator. + :param goal_validators: The goal validators to combine. + :param combined_name: The name of the combined goal validator. + return: The combined goal validator. + """ + + class CombinedGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[Any], goal_value: List[Any], + current_value_getter: Callable[[], List[Any]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, + acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return (sum([goal_validator.calculate_error(value_1, value_2) + for goal_validator, value_1, value_2 in zip(goal_validators, iterable_1, iterable_2)]) / + len(goal_validators)) + + return CombinedGoalValidator([goal_validator.initial_value for goal_validator in goal_validators], + [goal_validator.goal_value for goal_validator in goal_validators], + [goal_validator.current_value_getter for goal_validator in goal_validators], + combined_name, 0.5) + + +class PositionGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class OrientationGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], + name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class MultiJointGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[float], goal_value: List[float], + current_value_getter: Callable[[], List[float]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + +class PoseGoalValidator(GoalValidator): + """ + A class to validate the goal by tracking the goal achievement progress for a pose goal. + """ + + def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return calculate_pose_error(pose_1, pose_2) + + +class MultiPoseGoalValidator(IterableGoalValidator): + def __init__(self, initial_value: List[Pose], goal_value: List[Pose], + current_value_getter: Callable[[], List[Pose]], name: str, + acceptable_percentage_of_goal_achieved: float = 0.1): + super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + + def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: + """ + Calculate the error between two iterables. + return: The error between the two iterables. + """ + return (sum([calculate_pose_error(pose_1, pose_2) for pose_1, pose_2 in zip(iterable_1, iterable_2)]) / + len(iterable_1)) + + +def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> float: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return (np.linalg.norm(np.array(pose_1.position_as_list()) - np.array(pose_2.position_as_list())) + + np.linalg.norm(np.array(pose_1.orientation_as_list()) - np.array(pose_2.orientation_as_list()))) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 030208b1f..e2af322f4 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os import unittest import psutil @@ -42,7 +43,6 @@ def setUpClass(cls): return cls.multiverse = Multiverse(simulation="pycram_test", is_prospection=False) - # cls.big_bowl = cls.spawn_big_bowl() @classmethod def tearDownClass(cls): @@ -164,8 +164,8 @@ def test_set_robot_position(self): self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) def test_attach_object(self): - milk = self.spawn_milk([1, 0, 0.1]) - cup = self.spawn_cup([1, 1, 0.1]) + milk = self.spawn_milk([1, 0.1, 0.1]) + cup = self.spawn_cup([1, 1.1, 0.1]) milk.attach(cup) self.assertTrue(cup in milk.attachments) milk_position = milk.get_position_as_list() From e2c744f5a530f50f1ed9bbd50ca47a201a661256 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 01:11:24 +0200 Subject: [PATCH 061/189] [Multiverse] set_joint_positions takes joint names. --- src/pycram/datastructures/world.py | 6 ++--- src/pycram/worlds/multiverse.py | 8 +++---- .../multiverse_functions/goal_validator.py | 22 +++++++++---------- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e2dbbdf23..30aa53045 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -508,9 +508,9 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() - return {self.robot.joints[move_base_joints.translation_x]: position_diff[0], - self.robot.joints[move_base_joints.translation_y]: position_diff[1], - self.robot.joints[move_base_joints.angular_z]: angle_diff} + return {move_base_joints.translation_x: position_diff[0], + move_base_joints.translation_y: position_diff[1], + move_base_joints.angular_z: angle_diff} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7971a0850..ab24715e0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -323,10 +323,10 @@ def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: while not goal_validator.goal_achieved: sleep(0.01) if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to"\ - f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}"\ - f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage"\ - f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" + msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to" \ + f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ + f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ + f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" logging.error(msg) raise TimeoutError(msg) current = goal_validator.current_value diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 423f5c5e5..1a66949fc 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,7 +1,7 @@ from abc import abstractmethod, ABC import numpy as np -from typing_extensions import List, Union, Any, Callable +from typing_extensions import List, Union, Any, Callable, Optional from pycram.datastructures.pose import Pose @@ -12,7 +12,7 @@ class GoalValidator(ABC): """ def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): self.initial_value: Any = initial_value self.current_value_getter: Callable[[], Any] = current_value_getter self.goal_value: Any = goal_value @@ -73,7 +73,7 @@ class SingleValueGoalValidator(GoalValidator): def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: @@ -87,7 +87,7 @@ def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int] class JointGoalValidator(SingleValueGoalValidator): def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -98,7 +98,7 @@ class IterableGoalValidator(GoalValidator): def __init__(self, initial_value: List[Any], goal_value: List[Any], current_value_getter: Callable[[], List[Any]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: @@ -120,7 +120,7 @@ def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_n class CombinedGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[Any], goal_value: List[Any], current_value_getter: Callable[[], List[Any]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -142,21 +142,21 @@ def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float class PositionGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) class OrientationGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: float = 0.1): + name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) class MultiJointGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[float], goal_value: List[float], current_value_getter: Callable[[], List[float]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) @@ -166,7 +166,7 @@ class PoseGoalValidator(GoalValidator): """ def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: @@ -180,7 +180,7 @@ def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: class MultiPoseGoalValidator(IterableGoalValidator): def __init__(self, initial_value: List[Pose], goal_value: List[Pose], current_value_getter: Callable[[], List[Pose]], name: str, - acceptable_percentage_of_goal_achieved: float = 0.1): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: From 18f3ff7810491cb6dca608d61a3a8f17cd6902f4 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 01:17:49 +0200 Subject: [PATCH 062/189] [Multiverse] no need to put all files in resources. Directories are enough. --- src/pycram/datastructures/world.py | 2 +- src/pycram/worlds/multiverse.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ec061d7b3..60ac5e45b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -137,7 +137,7 @@ class World(StateEntity, ABC): the objects. """ - data_directory: List[str] = [resources_path, os.path.join(resources_path, 'robots')] + data_directory: List[str] = [resources_path] """ Global reference for the data directories, this is used to search for the description files of the robot, the objects, and the cached files. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index cccb57320..63183181f 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -102,8 +102,7 @@ def _make_sure_multiverse_resources_are_added(self): if not self.added_multiverse_resources: World.cache_manager.clear_cache() dirname = find_multiverse_resources_path() - resources_paths = get_resource_paths(dirname) - World.data_directory = resources_paths + self.data_directory + World.data_directory = [dirname] + self.data_directory World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True From ff85251cf226f0dfaaa183ae59573ee0035528f2 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 01:50:08 +0200 Subject: [PATCH 063/189] [Multiverse] get_child_object_pose in attachment. --- src/pycram/world_concepts/constraints.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 955336925..5843db56e 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -30,6 +30,14 @@ def __init__(self, self.child_to_constraint = child_to_constraint self._parent_to_child = None + def get_child_object_pose(self) -> Pose: + """ + Returns the pose of the child object. + + :return: The pose of the child object. + """ + return self.child_link.object.pose + def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: """ Returns the pose of the child object given the parent pose. From aa90866b18aa4504c57bd5fbee3ba0f3c925decf Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 22:11:41 +0200 Subject: [PATCH 064/189] [Multiverse] goal_validator.py is tested with bullet_world and is working. --- src/pycram/datastructures/pose.py | 13 +- src/pycram/world_concepts/world_object.py | 1 - src/pycram/worlds/multiverse.py | 3 +- .../multiverse_functions/error_checkers.py | 224 ++++++++++++++++++ .../multiverse_functions/goal_validator.py | 217 ++++++----------- test/test_error_checkers.py | 131 ++++++++++ test/test_goal_validator.py | 224 ++++++++++++++++++ 7 files changed, 656 insertions(+), 157 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/error_checkers.py create mode 100644 test/test_error_checkers.py create mode 100644 test/test_goal_validator.py diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 8e57cc0ba..52e2da02f 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -3,7 +3,7 @@ import math import datetime -from typing_extensions import List, Union, Optional +from typing_extensions import List, Union, Optional, Sized import numpy as np import rospy @@ -144,14 +144,17 @@ def orientation(self, value) -> None: :param value: New orientation, either a list or geometry_msgs/Quaternion """ - if not isinstance(value, list) and not isinstance(value, tuple) and not isinstance(value, GeoQuaternion): - rospy.logwarn("Orientation can only be a list or geometry_msgs/Quaternion") + if not isinstance(value, Sized) and not isinstance(value, GeoQuaternion): + rospy.logwarn("Orientation can only be an iterable (list, tuple, ...etc.) or a geometry_msgs/Quaternion") return - if isinstance(value, list) or isinstance(value, tuple) and len(value) == 4: + if isinstance(value, Sized) and len(value) == 4: orientation = np.array(value) - else: + elif isinstance(value, GeoQuaternion): orientation = np.array([value.x, value.y, value.z, value.w]) + else: + rospy.logerr("Orientation has to be a list or geometry_msgs/Quaternion") + raise TypeError("Orientation has to be a list or geometry_msgs/Quaternion") # This is used instead of np.linalg.norm since numpy is too slow on small arrays self.pose.orientation = get_normalized_quaternion(orientation) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index f64baa339..843c70c3a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -92,7 +92,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) - def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the target poses of the attached objects. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ab24715e0..511f28ddd 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -255,7 +255,8 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po param initial_poses: The list of initial poses of the attached objects. """ target_poses = obj.get_target_poses_of_attached_objects() - self._wait_until_all_pose_goals_are_achieved(list(target_poses.keys()), list(target_poses.values()), + body_names = [obj.name for obj in target_poses.keys()] + self._wait_until_all_pose_goals_are_achieved(body_names, list(target_poses.values()), initial_poses) def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py new file mode 100644 index 000000000..94af1efc7 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -0,0 +1,224 @@ +from abc import ABC, abstractmethod +from collections.abc import Iterable + +import numpy as np +from tf.transformations import quaternion_multiply, quaternion_inverse +from typing_extensions import List, Union, Optional, Any, Iterable as TypingIterable, Sized + +from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Pose + + +class ErrorChecker(ABC): + """ + An abstract class that resembles an error checker. It has two main methods, one for calculating the error between + two values and another for checking if the error is acceptable. + """ + def __init__(self, acceptable_error: Union[float, TypingIterable[float]], is_iterable: Optional[bool] = False): + """ + Initialize the error checker. + :param acceptable_error: The acceptable error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ + self.acceptable_error = np.array(acceptable_error) + self.is_iterable = is_iterable + + def get_acceptable_error(self, tile_to_match: Optional[Sized] = None) -> np.ndarray: + """ + Get the acceptable error. + :return: The acceptable error. + """ + if tile_to_match is not None and self.is_iterable: + return self.tiled_acceptable_error(tile_to_match) + else: + return self.acceptable_error + + def tiled_acceptable_error(self, tile_to_match: Sized) -> np.ndarray: + """ + Tile the acceptable error to match the length of the error. + :param tile_to_match: The object to match the length of the error. + :return: The tiled acceptable error. + """ + return np.tile(np.array(self.acceptable_error).flatten(), len(tile_to_match) // self.acceptable_error.size) + + @abstractmethod + def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + pass + + def calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: + """ + Calculate the error between two values. + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. + """ + if self.is_iterable: + return [self._calculate_error(v1, v2) for v1, v2 in zip(value_1, value_2)] + else: + return self._calculate_error(value_1, value_2) + + def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: + """ + Check if the error is acceptable. + :param value_1: The first value. + :param value_2: The second value. + :return: Whether the error is acceptable. + """ + error = self.calculate_error(value_1, value_2) + if self.is_iterable: + error = np.array(error).flatten() + return np.all(error <= self.tiled_acceptable_error(error)) + else: + return is_error_acceptable(error, self.acceptable_error) + + +class PoseErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Union[float, TypingIterable[float]] = (1e-3, np.pi / 180), + is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> List[float]: + """ + Calculate the error between two poses. + """ + return calculate_pose_error(value_1, value_2) + + +class PositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two positions. + """ + return calculate_position_error(value_1, value_2) + + +class OrientationErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two quaternions. + """ + return calculate_orientation_error(value_1, value_2) + + +class MultiJointPositionErrorChecker(ErrorChecker): + + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[List[float]] = None): + self.joint_types = joint_types + if acceptable_error is None: + acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] + super().__init__(acceptable_error, True) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +class RevoluteJointPositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +class PrismaticJointPositionErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + def _calculate_error(self, value_1: Any, value_2: Any) -> float: + """ + Calculate the error between two joint positions. + """ + return calculate_joint_position_error(value_1, value_2) + + +def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> List[float]: + """ + Calculate the error between two poses. + return: The error between the two poses. + """ + return [calculate_position_error(pose_1.position_as_list(), pose_2.position_as_list()), + calculate_orientation_error(pose_1.orientation_as_list(), pose_2.orientation_as_list())] + + +def calculate_position_error(position_1: List[float], position_2: List[float]) -> float: + """ + Calculate the error between two positions. + return: The error between the two positions. + """ + return np.linalg.norm(np.array(position_1) - np.array(position_2)) + + +def calculate_orientation_error(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculate the error between two quaternions. + return: The error between the two quaternions. + """ + return calculate_angle_between_quaternions(quat_1, quat_2) + + +def calculate_joint_position_error(joint_position_1: float, joint_position_2: float) -> float: + """ + Calculate the error between two joint positions. + return: The error between the two joint positions. + """ + return abs(joint_position_1 - joint_position_2) + + +def is_error_acceptable(error: Union[float, TypingIterable[float]], + acceptable_error: Union[float, TypingIterable[float]]) -> bool: + """ + Check if the error is acceptable. + :param error: The error. + :param acceptable_error: The acceptable error. + return: Whether the error is acceptable. + """ + if isinstance(error, Iterable): + return all([error_i <= acceptable_error_i for error_i, acceptable_error_i in zip(error, acceptable_error)]) + else: + return error <= acceptable_error + + +def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float]) -> float: + """ + Calculates the angle between two quaternions. + :param quat_1: The first quaternion. + :param quat_2: The second quaternion. + :return: A float value that represents the angle between the two quaternions. + """ + quat_diff = calculate_quaternion_difference(quat_1, quat_2) + quat_diff_angle = 2 * np.arctan2(np.linalg.norm(quat_diff[0:3]), quat_diff[3]) + return quat_diff_angle + + +def calculate_quaternion_difference(quat_1: List[float], quat_2: List[float]) -> List[float]: + """ + Calculates the quaternion difference. + :param quat_1: The quaternion of the object at the first time step. + :param quat_2: The quaternion of the object at the second time step. + :return: A list of float values that represent the quaternion difference. + """ + quat_diff = quaternion_multiply(quaternion_inverse(quat_1), quat_2) + return quat_diff diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 1a66949fc..4a6bc38b0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,30 +1,35 @@ -from abc import abstractmethod, ABC - import numpy as np -from typing_extensions import List, Union, Any, Callable, Optional +from typing_extensions import Any, Callable, Optional, List, Union -from pycram.datastructures.pose import Pose +from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker -class GoalValidator(ABC): +class GoalValidator: """ A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, initial_value: Any, goal_value: Any, current_value_getter: Callable[[], Any], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - self.initial_value: Any = initial_value - self.current_value_getter: Callable[[], Any] = current_value_getter - self.goal_value: Any = goal_value - self.name: str = name - self.acceptable_percentage_of_goal_achieved: float = acceptable_percentage_of_goal_achieved - self.initial_error: float = self.calculate_initial_error() - - def calculate_initial_error(self) -> float: + def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, + initial_value: Any = None, + acceptable_percentage_of_goal_achieved: Optional[float] = None): """ - Calculate the initial error. + Initialize the goal validator. + :param goal_value: The goal value. + :param current_value_getter: The current value getter. + :param error_checker: The error checker object. + :param initial_value: The initial value. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be + used to check if this percentage is achieved instead of the complete goal. """ - return self.calculate_error(self.goal_value, self.initial_value) + self.error_checker: ErrorChecker = error_checker + self.current_value_getter: Callable[[], Any] = current_value_getter + self.goal_value: Any = goal_value + if initial_value is None: + self.initial_error: List[Union[float, List[float]]] = self.current_error + else: + self.initial_error: List[Union[float, List[float]]] = self.calculate_error(goal_value, initial_value) + self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved + self.acceptable_error = self.error_checker.get_acceptable_error(self.initial_error) @property def current_value(self) -> Any: @@ -34,168 +39,80 @@ def current_value(self) -> Any: return self.current_value_getter() @property - def current_error(self) -> float: + def current_error(self) -> np.ndarray: """ Calculate the current error. """ return self.calculate_error(self.goal_value, self.current_value) - @abstractmethod - def calculate_error(self, value_1: Any, value_2: Any) -> float: + def calculate_error(self, value_1: Any, value_2: Any) -> np.ndarray: """ Calculate the error between two values. """ - pass + return np.array(self.error_checker.calculate_error(value_1, value_2)).flatten() @property def percentage_of_goal_achieved(self) -> float: """ Calculate the percentage of goal achieved. """ - if self.initial_error > 1e-3: - return 1 - self.current_error / self.initial_error - else: + percent_array = 1 - self.relative_current_error / self.relative_initial_error + percent_array_filtered = percent_array[self.relative_initial_error > self.acceptable_error] + if len(percent_array_filtered) == 0: return 1 + else: + return np.mean(percent_array_filtered) @property - def goal_achieved(self) -> bool: + def actual_percentage_of_goal_achieved(self) -> float: """ - Check if the goal is achieved. - return: Whether the goal is achieved. + Calculate the percentage of goal achieved. """ - return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved - - -class SingleValueGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for a single value. - """ - - def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], - current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) + percent_array_filtered = percent_array[self.initial_error > self.acceptable_error] + if len(percent_array_filtered) == 0: + return 1 + else: + return np.mean(percent_array_filtered) - def calculate_error(self, value_1: Union[float, int], value_2: Union[float, int]) -> Union[float, int]: + @property + def relative_current_error(self) -> np.ndarray: """ - Calculate the error between two values. - return: The error between the two values. + Get the relative current error. """ - return abs(value_1 - value_2) - - -class JointGoalValidator(SingleValueGoalValidator): - def __init__(self, initial_value: Union[float, int], goal_value: Union[float, int], - current_value_getter: Callable[[], Union[float, int]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + return self.get_relative_error(self.current_error, threshold=0) - -class IterableGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for an iterable goal. - """ - - def __init__(self, initial_value: List[Any], goal_value: List[Any], - current_value_getter: Callable[[], List[Any]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: + @property + def relative_initial_error(self) -> np.ndarray: """ - Calculate the error between two iterables. - return: The error between the two iterables. + Get the relative initial error. """ - return np.linalg.norm(np.array(iterable_1) - np.array(iterable_2)) - + return self.get_relative_error(self.initial_error) -def get_combined_goal_validator(goal_validators: List[GoalValidator], combined_name: str): - """ - Get a combined goal validator. - :param goal_validators: The goal validators to combine. - :param combined_name: The name of the combined goal validator. - return: The combined goal validator. - """ - - class CombinedGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[Any], goal_value: List[Any], - current_value_getter: Callable[[], List[Any]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, - acceptable_percentage_of_goal_achieved) - - def calculate_error(self, iterable_1: List[Any], iterable_2: List[Any]) -> float: - """ - Calculate the error between two iterables. - return: The error between the two iterables. - """ - return (sum([goal_validator.calculate_error(value_1, value_2) - for goal_validator, value_1, value_2 in zip(goal_validators, iterable_1, iterable_2)]) / - len(goal_validators)) - - return CombinedGoalValidator([goal_validator.initial_value for goal_validator in goal_validators], - [goal_validator.goal_value for goal_validator in goal_validators], - [goal_validator.current_value_getter for goal_validator in goal_validators], - combined_name, 0.5) - - -class PositionGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class OrientationGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], - name: str, acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class MultiJointGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[float], goal_value: List[float], - current_value_getter: Callable[[], List[float]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - -class PoseGoalValidator(GoalValidator): - """ - A class to validate the goal by tracking the goal achievement progress for a pose goal. - """ - - def __init__(self, initial_value: Pose, goal_value: Pose, current_value_getter: Callable[[], Pose], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) - - def calculate_error(self, pose_1: Pose, pose_2: Pose) -> float: + def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: """ - Calculate the error between two poses. - return: The error between the two poses. + Get the relative error by comparing the error with the acceptable error and filtering out the errors that are + less than the threshold. + :param error: The error. + :param threshold: The threshold. """ - return calculate_pose_error(pose_1, pose_2) - - -class MultiPoseGoalValidator(IterableGoalValidator): - def __init__(self, initial_value: List[Pose], goal_value: List[Pose], - current_value_getter: Callable[[], List[Pose]], name: str, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.01): - super().__init__(initial_value, goal_value, current_value_getter, name, acceptable_percentage_of_goal_achieved) + return np.maximum(error-self.acceptable_error, threshold) - def calculate_error(self, iterable_1: List[Pose], iterable_2: List[Pose]) -> float: + @property + def goal_achieved(self) -> bool: """ - Calculate the error between two iterables. - return: The error between the two iterables. + Check if the goal is achieved. + return: Whether the goal is achieved. """ - return (sum([calculate_pose_error(pose_1, pose_2) for pose_1, pose_2 in zip(iterable_1, iterable_2)]) / - len(iterable_1)) - + if self.acceptable_percentage_of_goal_achieved is None: + return self.is_error_acceptable + else: + return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved -def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> float: - """ - Calculate the error between two poses. - return: The error between the two poses. - """ - return (np.linalg.norm(np.array(pose_1.position_as_list()) - np.array(pose_2.position_as_list())) - + np.linalg.norm(np.array(pose_1.orientation_as_list()) - np.array(pose_2.orientation_as_list()))) + @property + def is_error_acceptable(self) -> bool: + """ + Check if the error is acceptable. + return: Whether the error is acceptable. + """ + return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py new file mode 100644 index 000000000..b7a459ff9 --- /dev/null +++ b/test/test_error_checkers.py @@ -0,0 +1,131 @@ +from unittest import TestCase + +import numpy as np +from tf.transformations import quaternion_from_euler + +from pycram.datastructures.enums import JointType +from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ + PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker + +from pycram.datastructures.pose import Pose + + +class TestErrorCheckers(TestCase): + @classmethod + def setUpClass(cls): + pass + + @classmethod + def tearDownClass(cls): + pass + + def tearDown(self): + pass + + def test_calculate_quaternion_error(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, 0.0) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = calculate_angle_between_quaternions(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + + def test_pose_error_checker(self): + pose_1 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + pose_2 = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) + error_checker = PoseErrorChecker() + error = error_checker.calculate_error(pose_1, pose_2) + self.assertEqual(error, [0.0, 0.0]) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + pose_2 = Pose([0, 1, np.sqrt(3)], quat) + error = error_checker.calculate_error(pose_1, pose_2) + self.assertAlmostEqual(error[0], 2, places=5) + self.assertEqual(error[1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi/360) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertTrue(error_checker.is_error_acceptable(pose_1, pose_2)) + quat = quaternion_from_euler(0, 0, np.pi / 179) + pose_2 = Pose([0, 0.0001, 0.0001], quat) + self.assertFalse(error_checker.is_error_acceptable(pose_1, pose_2)) + + def test_position_error_checker(self): + position_1 = [0.0, 0.0, 0.0] + position_2 = [0.0, 0.0, 0.0] + error_checker = PositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = [1.0, 1.0, 1.0] + error = error_checker.calculate_error(position_1, position_2) + self.assertAlmostEqual(error, np.sqrt(3), places=5) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_orientation_error_checker(self): + quat_1 = [0.0, 0.0, 0.0, 1.0] + quat_2 = [0.0, 0.0, 0.0, 1.0] + error_checker = OrientationErrorChecker() + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(quat_1, quat_2)) + quat_2 = quaternion_from_euler(0, 0, np.pi/2) + error = error_checker.calculate_error(quat_1, quat_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(quat_1, quat_2)) + + def test_revolute_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = RevoluteJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = np.pi/2 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_prismatic_joint_position_error_checker(self): + position_1 = 0.0 + position_2 = 0.0 + error_checker = PrismaticJointPositionErrorChecker() + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 0.0) + self.assertTrue(error_checker.is_error_acceptable(position_1, position_2)) + position_2 = 1.0 + error = error_checker.calculate_error(position_1, position_2) + self.assertEqual(error, 1.0) + self.assertFalse(error_checker.is_error_acceptable(position_1, position_2)) + + def test_list_of_poses_error_checker(self): + poses_1 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + poses_2 = [Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])] + error_checker = PoseErrorChecker(is_iterable=True) + error = error_checker.calculate_error(poses_1, poses_2) + self.assertEqual(error, [[0.0, 0.0], [0.0, 0.0]]) + self.assertTrue(error_checker.is_error_acceptable(poses_1, poses_2)) + quat = quaternion_from_euler(0, np.pi/2, 0) + poses_2 = [Pose([0, 1, np.sqrt(3)], quat), + Pose([0, 1, np.sqrt(3)], quat)] + error = error_checker.calculate_error(poses_1, poses_2) + self.assertAlmostEqual(error[0][0], 2, places=5) + self.assertEqual(error[0][1], np.pi/2) + self.assertAlmostEqual(error[1][0], 2, places=5) + self.assertEqual(error[1][1], np.pi/2) + self.assertFalse(error_checker.is_error_acceptable(poses_1, poses_2)) + + def test_multi_joint_error_checker(self): + positions_1 = [0.0, 0.0] + positions_2 = [np.pi/2, 0.1] + joint_types = [JointType.REVOLUTE, JointType.PRISMATIC] + error_checker = MultiJointPositionErrorChecker(joint_types) + error = error_checker.calculate_error(positions_1, positions_2) + self.assertEqual(error, [np.pi/2, 0.1]) + self.assertFalse(error_checker.is_error_acceptable(positions_1, positions_2)) + positions_2 = [np.pi/180, 0.0001] + self.assertTrue(error_checker.is_error_acceptable(positions_1, positions_2)) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py new file mode 100644 index 000000000..6ab6a289f --- /dev/null +++ b/test/test_goal_validator.py @@ -0,0 +1,224 @@ +from tf.transformations import quaternion_from_euler + +from bullet_world_testcase import BulletWorldTestCase +import numpy as np + +from pycram.datastructures.enums import JointType +from pycram.robot_description import RobotDescription +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator +from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ + MultiJointPositionErrorChecker + +from pycram.datastructures.pose import Pose + + +class TestGoalValidator(BulletWorldTestCase): + + def test_single_pose_goal(self): + milk_goal_pose = Pose([1.3, 1.5, 0.9]) + goal_validator = GoalValidator(milk_goal_pose, self.milk.get_pose, PoseErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.milk.set_pose(milk_goal_pose) + self.assertEqual(self.milk.get_pose(), milk_goal_pose) + self.assertTrue(goal_validator.goal_achieved) + print(goal_validator.current_error) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + + def test_single_position_goal(self): + cereal_goal_position = [1.3, 1.5, 0.95] + goal_validator = GoalValidator(cereal_goal_position, self.cereal.get_position_as_list, PositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, 0.8) + self.cereal.set_position(cereal_goal_position) + self.assertEqual(self.cereal.get_position_as_list(), cereal_goal_position) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.current_error, 0) + + def test_single_orientation_goal(self): + cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) + goal_validator = GoalValidator(cereal_goal_orientation, self.cereal.get_orientation_as_list, + OrientationErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, [np.pi/2]) + self.cereal.set_orientation(cereal_goal_orientation) + for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): + self.assertAlmostEqual(v1, v2, places=5) + self.assertTrue(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + + def test_single_revolute_joint_position_goal(self): + goal_joint_position = -np.pi/4 + goal_validator = GoalValidator(goal_joint_position, + lambda: self.robot.get_joint_position('l_shoulder_lift_joint'), + RevoluteJointPositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position*percent) + self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position*percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + + def test_single_prismatic_joint_position_goal(self): + goal_joint_position = 0.2 + torso = RobotDescription.current_robot_description.torso_joint + goal_validator = GoalValidator(goal_joint_position, + lambda: self.robot.get_joint_position(torso), + PrismaticJointPositionErrorChecker()) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) + + for percent in [0.5, 1]: + self.robot.set_joint_position(torso, goal_joint_position*percent) + self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position*percent) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + + def test_multi_joint_goal(self): + goal_joint_positions = [0.2, -np.pi/4] + joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = GoalValidator(goal_joint_positions, + lambda: [self.robot.get_joint_position('torso_lift_joint'), + self.robot.get_joint_position('l_shoulder_lift_joint')], + MultiJointPositionErrorChecker(joint_types)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_positions = [0.2*percent, -np.pi/4*percent] + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions))) + self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], + atol=0.001)) + self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi/4)*(1-percent), places=5) + + def test_list_of_poses_goal(self): + position_goal = [0.0, 1.0, 0.0] + orientation_goal = np.array([0, 0, np.pi/2]) + poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), + Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] + goal_validator = GoalValidator(poses_goal, lambda: [self.robot.get_pose(), self.robot.get_pose()], + PoseErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + current_pose_goal = Pose([0.0, 1.0*percent, 0.0], + quaternion_from_euler(*current_orientation_goal.tolist())) + self.robot.set_pose(current_pose_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_pose_goal.position_as_list(), + atol=0.001)) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), current_pose_goal.orientation_as_list(), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1-percent) /2, places=5) + self.assertAlmostEqual(goal_validator.current_error[2], (1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1-percent) /2, places=5) + + def test_list_of_positions_goal(self): + position_goal = [0.0, 1.0, 0.0] + positions_goal = [position_goal, position_goal] + goal_validator = GoalValidator(positions_goal, lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()], + PositionErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) + + for percent in [0.5, 1]: + current_position_goal = [0.0, 1.0*percent, 0.0] + self.robot.set_position(current_position_goal) + self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_position_goal, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 1-percent, places=5) + + def test_list_of_orientations_goal(self): + orientation_goal = np.array([0, 0, np.pi/2]) + orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), + quaternion_from_euler(*orientation_goal.tolist())] + goal_validator = GoalValidator(orientations_goals, lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()], + OrientationErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) + + for percent in [0.5, 1]: + current_orientation_goal = orientation_goal * percent + self.robot.set_orientation(quaternion_from_euler(*current_orientation_goal.tolist())) + self.assertTrue(np.allclose(self.robot.get_orientation_as_list(), + quaternion_from_euler(*current_orientation_goal.tolist()), + atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], np.pi*(1-percent)/2, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi*(1-percent)/2, places=5) + + def test_list_of_revolute_joint_positions_goal(self): + goal_joint_position = -np.pi/4 + goal_joint_positions = [goal_joint_position, goal_joint_position] + goal_validator = GoalValidator(goal_joint_positions, + lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), + self.robot.get_joint_position('l_shoulder_lift_joint')], + RevoluteJointPositionErrorChecker(is_iterable=True)) + self.assertFalse(goal_validator.goal_achieved) + self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertTrue(np.allclose(goal_validator.current_error, + np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) + + for percent in [0.5, 1]: + current_joint_position = goal_joint_position * percent + self.robot.set_joint_position('l_shoulder_lift_joint', current_joint_position) + self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_position, atol=0.001)) + if percent == 1: + self.assertTrue(goal_validator.goal_achieved) + else: + self.assertFalse(goal_validator.goal_achieved) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position)*(1-percent), places=5) + From 516eaecfe8c8a81a8a87b9738635d232fb9aec6a Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 22 Jul 2024 23:13:59 +0200 Subject: [PATCH 065/189] [Multiverse] debugging goal_validator in multiverse. --- src/pycram/worlds/multiverse.py | 39 ++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 511f28ddd..d19b84bf4 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,11 +4,12 @@ import numpy as np from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Iterable, Tuple +from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager -from .multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ - MultiPoseGoalValidator, JointGoalValidator, MultiJointGoalValidator +from .multiverse_functions.error_checkers import PoseErrorChecker, MultiJointPositionErrorChecker, \ + RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker +from .multiverse_functions.goal_validator import GoalValidator from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -197,13 +198,15 @@ def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Jo self._wait_until_goal_is_achieved(goal_validator) def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> MultiJointGoalValidator: + initial_joint_positions: Dict[Joint, float]) -> GoalValidator: joints = list(joint_positions.keys()) + joint_types = [joint.type for joint in joints] target_joint_positions = list(joint_positions.values()) initial_joint_positions = list(initial_joint_positions.values()) - goal_validator = MultiJointGoalValidator(initial_joint_positions, target_joint_positions, - lambda: list(self.get_multiple_joint_positions(joints).values()), - joints[0].object_name + "_joint_goal") + goal_validator = GoalValidator(target_joint_positions, + lambda: list(self.get_multiple_joint_positions(joints).values()), + MultiJointPositionErrorChecker(joint_types), + initial_value=initial_joint_positions) return goal_validator def get_joint_position(self, joint: Joint) -> float: @@ -221,8 +224,10 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, initial_joint_position: float) -> None: - goal_validator = JointGoalValidator(initial_joint_position, joint_position, - lambda: self.get_joint_position(joint), joint.name + "_joint_goal") + error_checker = RevoluteJointPositionErrorChecker() if joint.type == JointType.REVOLUTE else \ + PrismaticJointPositionErrorChecker() + goal_validator = GoalValidator(joint_position, lambda: self.get_joint_position(joint), + error_checker, initial_value=initial_joint_position) self._wait_until_goal_is_achieved(goal_validator) def get_link_pose(self, link: Link) -> Pose: @@ -260,16 +265,14 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po initial_poses) def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], - initial_poses: List[Pose], - name: Optional[str] = "all_poses_goal") -> None: + initial_poses: List[Pose]) -> None: """ Wait until all poses are set to the desired poses. param poses: The dictionary of the desired poses param initial_poses: The dictionary of the initial poses - param name: The name of the goal. """ - goal_validator = MultiPoseGoalValidator(initial_poses, poses, - lambda: list(self._get_multiple_body_poses(body_names).values()), name) + goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), + PoseErrorChecker(), initial_value=initial_poses) self._wait_until_goal_is_achieved(goal_validator) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: @@ -310,8 +313,8 @@ def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, i param target_pose: The target pose of the body. param initial_pose: The initial pose of the body. """ - goal_validator = PoseGoalValidator(initial_pose, target_pose, lambda: self._get_body_pose(body_name), - body_name + "_pose_goal") + goal_validator = GoalValidator(target_pose, lambda: self._get_body_pose(body_name), + PoseErrorChecker(), initial_value=initial_pose) self._wait_until_goal_is_achieved(goal_validator) def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: @@ -324,8 +327,8 @@ def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: while not goal_validator.goal_achieved: sleep(0.01) if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve {goal_validator.name} from {goal_validator.initial_value} to" \ - f" {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ + msg = f"Failed to achieve goal from initial error {goal_validator.initial_error} with" \ + f" goal {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" logging.error(msg) From b27049803c31b1b8df0ed226c4668431840212aa Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 23 Jul 2024 03:51:47 +0200 Subject: [PATCH 066/189] [Multiverse] debugging goal_validator in multiverse, attach is working. --- src/pycram/datastructures/world.py | 2 +- src/pycram/world_concepts/world_object.py | 3 ++- src/pycram/worlds/multiverse.py | 2 +- src/pycram/worlds/multiverse_communication/clients.py | 4 ++-- src/pycram/worlds/multiverse_functions/goal_validator.py | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 30aa53045..41293c562 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -951,7 +951,7 @@ def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]] :param exclude_objects: A list of objects that should not be removed. """ self.reset_world() - objs_copy = copy(self.objects) + objs_copy = [obj for obj in self.objects] exclude_objects = [] if exclude_objects is None else exclude_objects [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 843c70c3a..a719fff8d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -456,7 +456,8 @@ def attach(self, if coincide_the_objects: parent_to_child_transform = Transform() else: - parent_to_child_transform = parent_link.get_transform_to_link(child_link) + # parent_to_child_transform = parent_link.get_transform_to_link(child_link) + parent_to_child_transform = None attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d19b84bf4..908a7143b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -272,7 +272,7 @@ def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: param initial_poses: The dictionary of the initial poses """ goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), - PoseErrorChecker(), initial_value=initial_poses) + PoseErrorChecker(is_iterable=True), initial_value=initial_poses) self._wait_until_goal_is_achieved(goal_validator) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 0feb5e1ff..6e3f9f113 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -306,8 +306,8 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List """ self.send_body_data_to_server(body_name, {"position": position, - "quaternion": orientation, - "relative_velocity": [0]*6}) + "quaternion": orientation} + ) def set_body_position(self, body_name: str, position: List[float]) -> None: """ diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 4a6bc38b0..db7852d8a 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -11,7 +11,7 @@ class GoalValidator: def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, initial_value: Any = None, - acceptable_percentage_of_goal_achieved: Optional[float] = None): + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. :param goal_value: The goal value. From 9d7f7390ae07b589176653bace59f0e755366757 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 15:16:41 +0200 Subject: [PATCH 067/189] [Multiverse] updated goal validator with register goal value function. --- .../multiverse_functions/error_checkers.py | 37 +++++----- .../multiverse_functions/goal_validator.py | 70 ++++++++++++++----- test/test_goal_validator.py | 51 ++++++++------ 3 files changed, 103 insertions(+), 55 deletions(-) diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index 94af1efc7..cf85425d9 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -3,7 +3,7 @@ import numpy as np from tf.transformations import quaternion_multiply, quaternion_inverse -from typing_extensions import List, Union, Optional, Any, Iterable as TypingIterable, Sized +from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose @@ -14,32 +14,34 @@ class ErrorChecker(ABC): An abstract class that resembles an error checker. It has two main methods, one for calculating the error between two values and another for checking if the error is acceptable. """ - def __init__(self, acceptable_error: Union[float, TypingIterable[float]], is_iterable: Optional[bool] = False): + def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterable: Optional[bool] = False): """ Initialize the error checker. :param acceptable_error: The acceptable error. :param is_iterable: Whether the error is iterable (i.e. list of errors). """ - self.acceptable_error = np.array(acceptable_error) + self.acceptable_error: np.ndarray = np.array(acceptable_error) + self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable - def get_acceptable_error(self, tile_to_match: Optional[Sized] = None) -> np.ndarray: + def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[float]] = None, + tile_to_match: Optional[Sized] = None,) -> None: """ - Get the acceptable error. - :return: The acceptable error. + Update the acceptable error with a new value, and tile it to match the length of the error if needed. """ + if new_acceptable_error is not None: + self.acceptable_error = np.array(new_acceptable_error) if tile_to_match is not None and self.is_iterable: - return self.tiled_acceptable_error(tile_to_match) - else: - return self.acceptable_error + self.update_tiled_acceptable_error(tile_to_match) - def tiled_acceptable_error(self, tile_to_match: Sized) -> np.ndarray: + def update_tiled_acceptable_error(self, tile_to_match: Sized) -> None: """ Tile the acceptable error to match the length of the error. :param tile_to_match: The object to match the length of the error. :return: The tiled acceptable error. """ - return np.tile(np.array(self.acceptable_error).flatten(), len(tile_to_match) // self.acceptable_error.size) + self.tiled_acceptable_error = np.tile(self.acceptable_error.flatten(), + len(tile_to_match) // self.acceptable_error.size) @abstractmethod def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: @@ -73,14 +75,17 @@ def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: error = self.calculate_error(value_1, value_2) if self.is_iterable: error = np.array(error).flatten() - return np.all(error <= self.tiled_acceptable_error(error)) + if self.tiled_acceptable_error is None or\ + len(error) != len(self.tiled_acceptable_error): + self.update_tiled_acceptable_error(error) + return np.all(error <= self.tiled_acceptable_error) else: return is_error_acceptable(error, self.acceptable_error) class PoseErrorChecker(ErrorChecker): - def __init__(self, acceptable_error: Union[float, TypingIterable[float]] = (1e-3, np.pi / 180), + def __init__(self, acceptable_error: Union[float, T_Iterable[float]] = (1e-3, np.pi / 180), is_iterable: Optional[bool] = False): super().__init__(acceptable_error, is_iterable) @@ -117,7 +122,7 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class MultiJointPositionErrorChecker(ErrorChecker): - def __init__(self, joint_types: List[JointType], acceptable_error: Optional[List[float]] = None): + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): self.joint_types = joint_types if acceptable_error is None: acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] @@ -187,8 +192,8 @@ def calculate_joint_position_error(joint_position_1: float, joint_position_2: fl return abs(joint_position_1 - joint_position_2) -def is_error_acceptable(error: Union[float, TypingIterable[float]], - acceptable_error: Union[float, TypingIterable[float]]) -> bool: +def is_error_acceptable(error: Union[float, T_Iterable[float]], + acceptable_error: Union[float, T_Iterable[float]]) -> bool: """ Check if the error is acceptable. :param error: The error. diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index db7852d8a..0483dc3cd 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,5 +1,5 @@ import numpy as np -from typing_extensions import Any, Callable, Optional, List, Union +from typing_extensions import Any, Callable, Optional, Union, Iterable from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker @@ -9,27 +9,65 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, goal_value: Any, current_value_getter: Callable[[], Any], error_checker: ErrorChecker, - initial_value: Any = None, + def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[], Any], acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. - :param goal_value: The goal value. + :param error_checker: The error checker. :param current_value_getter: The current value getter. - :param error_checker: The error checker object. - :param initial_value: The initial value. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be used to check if this percentage is achieved instead of the complete goal. """ self.error_checker: ErrorChecker = error_checker self.current_value_getter: Callable[[], Any] = current_value_getter - self.goal_value: Any = goal_value + self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved + self.goal_value: Any = None + self.initial_error: Optional[np.ndarray] = None + + @property + def _acceptable_error(self) -> np.ndarray: + """ + Get the acceptable error. + """ + if self.error_checker.is_iterable: + return self.tiled_acceptable_error + else: + return self.acceptable_error + + @property + def acceptable_error(self) -> np.ndarray: + """ + Get the acceptable error. + """ + return self.error_checker.acceptable_error + + @property + def tiled_acceptable_error(self) -> Optional[np.ndarray]: + """ + Get the tiled acceptable error. + """ + return self.error_checker.tiled_acceptable_error + + def register_goal_value(self, goal_value: Any, initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None): + """ + Register the goal value. + :param goal_value: The goal value. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + self.goal_value = goal_value + self.update_initial_error(goal_value, initial_value=initial_value) + self.error_checker.update_acceptable_error(acceptable_error, self.initial_error) + + def update_initial_error(self, goal_value: Any, initial_value: Optional[Any] = None) -> None: + """ + Calculate the initial error. + """ if initial_value is None: - self.initial_error: List[Union[float, List[float]]] = self.current_error + self.initial_error: np.ndarray = self.current_error else: - self.initial_error: List[Union[float, List[float]]] = self.calculate_error(goal_value, initial_value) - self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved - self.acceptable_error = self.error_checker.get_acceptable_error(self.initial_error) + self.initial_error: np.ndarray = self.calculate_error(goal_value, initial_value) @property def current_value(self) -> Any: @@ -57,7 +95,7 @@ def percentage_of_goal_achieved(self) -> float: Calculate the percentage of goal achieved. """ percent_array = 1 - self.relative_current_error / self.relative_initial_error - percent_array_filtered = percent_array[self.relative_initial_error > self.acceptable_error] + percent_array_filtered = percent_array[self.relative_initial_error > self._acceptable_error] if len(percent_array_filtered) == 0: return 1 else: @@ -69,7 +107,7 @@ def actual_percentage_of_goal_achieved(self) -> float: Calculate the percentage of goal achieved. """ percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) - percent_array_filtered = percent_array[self.initial_error > self.acceptable_error] + percent_array_filtered = percent_array[self.initial_error > self._acceptable_error] if len(percent_array_filtered) == 0: return 1 else: @@ -96,7 +134,7 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n :param error: The error. :param threshold: The threshold. """ - return np.maximum(error-self.acceptable_error, threshold) + return np.maximum(error-self._acceptable_error, threshold) @property def goal_achieved(self) -> bool: @@ -105,12 +143,12 @@ def goal_achieved(self) -> bool: return: Whether the goal is achieved. """ if self.acceptable_percentage_of_goal_achieved is None: - return self.is_error_acceptable + return self.is_current_error_acceptable else: return self.percentage_of_goal_achieved >= self.acceptable_percentage_of_goal_achieved @property - def is_error_acceptable(self) -> bool: + def is_current_error_acceptable(self) -> bool: """ Check if the error is acceptable. return: Whether the error is acceptable. diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 6ab6a289f..f2c115315 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -17,7 +17,8 @@ class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): milk_goal_pose = Pose([1.3, 1.5, 0.9]) - goal_validator = GoalValidator(milk_goal_pose, self.milk.get_pose, PoseErrorChecker()) + goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) + goal_validator.register_goal_value(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) @@ -32,7 +33,8 @@ def test_single_pose_goal(self): def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] - goal_validator = GoalValidator(cereal_goal_position, self.cereal.get_position_as_list, PositionErrorChecker()) + goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) + goal_validator.register_goal_value(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) @@ -44,8 +46,8 @@ def test_single_position_goal(self): def test_single_orientation_goal(self): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) - goal_validator = GoalValidator(cereal_goal_orientation, self.cereal.get_orientation_as_list, - OrientationErrorChecker()) + goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) + goal_validator.register_goal_value(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, [np.pi/2]) @@ -58,9 +60,9 @@ def test_single_orientation_goal(self): def test_single_revolute_joint_position_goal(self): goal_joint_position = -np.pi/4 - goal_validator = GoalValidator(goal_joint_position, - lambda: self.robot.get_joint_position('l_shoulder_lift_joint'), - RevoluteJointPositionErrorChecker()) + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), + lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) + goal_validator.register_goal_value(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -78,9 +80,9 @@ def test_single_revolute_joint_position_goal(self): def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(goal_joint_position, - lambda: self.robot.get_joint_position(torso), - PrismaticJointPositionErrorChecker()) + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), + lambda: self.robot.get_joint_position(torso)) + goal_validator.register_goal_value(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -99,10 +101,10 @@ def test_multi_joint_goal(self): goal_joint_positions = [0.2, -np.pi/4] joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] - goal_validator = GoalValidator(goal_joint_positions, + goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), lambda: [self.robot.get_joint_position('torso_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')], - MultiJointPositionErrorChecker(joint_types)) + self.robot.get_joint_position('l_shoulder_lift_joint')]) + goal_validator.register_goal_value(goal_joint_positions) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) @@ -127,8 +129,9 @@ def test_list_of_poses_goal(self): orientation_goal = np.array([0, 0, np.pi/2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] - goal_validator = GoalValidator(poses_goal, lambda: [self.robot.get_pose(), self.robot.get_pose()], - PoseErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), + lambda: [self.robot.get_pose(), self.robot.get_pose()]) + goal_validator.register_goal_value(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) @@ -155,8 +158,9 @@ def test_list_of_poses_goal(self): def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] - goal_validator = GoalValidator(positions_goal, lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()], - PositionErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), + lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) + goal_validator.register_goal_value(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) @@ -177,9 +181,10 @@ def test_list_of_orientations_goal(self): orientation_goal = np.array([0, 0, np.pi/2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), quaternion_from_euler(*orientation_goal.tolist())] - goal_validator = GoalValidator(orientations_goals, lambda: [self.robot.get_orientation_as_list(), - self.robot.get_orientation_as_list()], - OrientationErrorChecker(is_iterable=True)) + goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), + lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + goal_validator.register_goal_value(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) @@ -201,10 +206,10 @@ def test_list_of_orientations_goal(self): def test_list_of_revolute_joint_positions_goal(self): goal_joint_position = -np.pi/4 goal_joint_positions = [goal_joint_position, goal_joint_position] - goal_validator = GoalValidator(goal_joint_positions, + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')], - RevoluteJointPositionErrorChecker(is_iterable=True)) + self.robot.get_joint_position('l_shoulder_lift_joint')]) + goal_validator.register_goal_value(goal_joint_positions) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, From 81b96ec219d42b3f3c0ad95d639433bdf5f63873 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 16:48:07 +0200 Subject: [PATCH 068/189] [Multiverse] updated register goal value function to also take current_value_getter_input. --- src/pycram/datastructures/world.py | 23 ++-- src/pycram/world_concepts/world_object.py | 9 ++ src/pycram/worlds/bullet_world.py | 3 + src/pycram/worlds/multiverse.py | 4 +- .../multiverse_functions/goal_validator.py | 25 ++-- test/test_goal_validator.py | 117 +++++++++--------- 6 files changed, 104 insertions(+), 77 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 41293c562..8051313e5 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -606,6 +606,21 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: """ pass + @abstractmethod + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + """ + Set the positions of multiple joints of an articulated object. + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + """ + pass + + @abstractmethod + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + """ + Get the positions of multiple joints of an articulated object. + """ + pass + @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose): """ @@ -1195,14 +1210,6 @@ def resume_world_sync(self) -> None: def __del__(self): self.exit() - @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: - """ - Set the positions of multiple joints of an articulated object. - :param joint_positions: A dictionary with joint objects as keys and joint positions as values. - """ - pass - class UseProspectionWorld: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a719fff8d..d08918b56 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -984,6 +984,15 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: rospy.logwarn(f"No joint of type {joint_type} found above link {link_name}") return container_joint + def get_multiple_joint_positions(self, joint_names: List[str]) -> Dict[str, float]: + """ + Returns the positions of multiple joints at once. + + :param joint_names: A list of joint names. + :return: A dictionary with the joint names as keys and the joint positions as values. + """ + return self.world.get_multiple_joint_positions([self.joints[joint_name] for joint_name in joint_names]) + def get_positions_of_all_joints(self) -> Dict[str, float]: """ Returns the positions of all joints of the object as a dictionary of joint names and joint positions. diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index f18753979..8be0deaec 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -171,6 +171,9 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> N for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) + def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: + return {joint.name: self.get_joint_position(joint) for joint in joints} + def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 908a7143b..b4aabd858 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -313,8 +313,8 @@ def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, i param target_pose: The target pose of the body. param initial_pose: The initial pose of the body. """ - goal_validator = GoalValidator(target_pose, lambda: self._get_body_pose(body_name), - PoseErrorChecker(), initial_value=initial_pose) + goal_validator = GoalValidator(PoseErrorChecker(), lambda: self._get_body_pose(body_name)) + goal_validator.register_goal(target_pose, initial_pose) self._wait_until_goal_is_achieved(goal_validator) def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 0483dc3cd..b6fedf2fb 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,5 +1,5 @@ import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, Protocol from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker @@ -9,20 +9,22 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[], Any], + def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[Optional[Any]], Any], acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. :param error_checker: The error checker. - :param current_value_getter: The current value getter. + :param current_value_getter: The current value getter function which takes an optional input and returns the + current value. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved, if given, will be used to check if this percentage is achieved instead of the complete goal. """ self.error_checker: ErrorChecker = error_checker - self.current_value_getter: Callable[[], Any] = current_value_getter + self.current_value_getter: Callable[[Optional[Any]], Any] = current_value_getter self.acceptable_percentage_of_goal_achieved: Optional[float] = acceptable_percentage_of_goal_achieved - self.goal_value: Any = None + self.goal_value: Optional[Any] = None self.initial_error: Optional[np.ndarray] = None + self.current_value_getter_input: Optional[Any] = None @property def _acceptable_error(self) -> np.ndarray: @@ -48,15 +50,19 @@ def tiled_acceptable_error(self) -> Optional[np.ndarray]: """ return self.error_checker.tiled_acceptable_error - def register_goal_value(self, goal_value: Any, initial_value: Optional[Any] = None, - acceptable_error: Optional[Union[float, Iterable[float]]] = None): + def register_goal(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None): """ Register the goal value. :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. :param initial_value: The initial value. :param acceptable_error: The acceptable error. """ self.goal_value = goal_value + self.current_value_getter_input = current_value_getter_input self.update_initial_error(goal_value, initial_value=initial_value) self.error_checker.update_acceptable_error(acceptable_error, self.initial_error) @@ -74,7 +80,10 @@ def current_value(self) -> Any: """ Get the current value. """ - return self.current_value_getter() + if self.current_value_getter_input is not None: + return self.current_value_getter(self.current_value_getter_input) + else: + return self.current_value_getter() @property def current_error(self) -> np.ndarray: diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index f2c115315..5f8c6b1a2 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -1,16 +1,14 @@ +import numpy as np from tf.transformations import quaternion_from_euler from bullet_world_testcase import BulletWorldTestCase -import numpy as np - from pycram.datastructures.enums import JointType +from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker - -from pycram.datastructures.pose import Pose +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator class TestGoalValidator(BulletWorldTestCase): @@ -18,7 +16,7 @@ class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): milk_goal_pose = Pose([1.3, 1.5, 0.9]) goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) - goal_validator.register_goal_value(milk_goal_pose) + goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) @@ -34,7 +32,7 @@ def test_single_pose_goal(self): def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) - goal_validator.register_goal_value(cereal_goal_position) + goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) @@ -45,12 +43,12 @@ def test_single_position_goal(self): self.assertEqual(goal_validator.current_error, 0) def test_single_orientation_goal(self): - cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi/2) + cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) - goal_validator.register_goal_value(cereal_goal_orientation) + goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertEqual(goal_validator.current_error, [np.pi/2]) + self.assertEqual(goal_validator.current_error, [np.pi / 2]) self.cereal.set_orientation(cereal_goal_orientation) for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): self.assertAlmostEqual(v1, v2, places=5) @@ -59,86 +57,87 @@ def test_single_orientation_goal(self): self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) def test_single_revolute_joint_position_goal(self): - goal_joint_position = -np.pi/4 + goal_joint_position = -np.pi / 4 goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) - goal_validator.register_goal_value(goal_joint_position) + goal_validator.register_goal(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: - self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position*percent) - self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position*percent) + self.robot.set_joint_position('l_shoulder_lift_joint', goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position('l_shoulder_lift_joint'), goal_joint_position * percent) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), lambda: self.robot.get_joint_position(torso)) - goal_validator.register_goal_value(goal_joint_position) + goal_validator.register_goal(goal_joint_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: - self.robot.set_joint_position(torso, goal_joint_position*percent) - self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position*percent) + self.robot.set_joint_position(torso, goal_joint_position * percent) + self.assertEqual(self.robot.get_joint_position(torso), goal_joint_position * percent) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) def test_multi_joint_goal(self): - goal_joint_positions = [0.2, -np.pi/4] + goal_joint_positions = np.array([0.2, -np.pi / 4]) joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), - lambda: [self.robot.get_joint_position('torso_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')]) - goal_validator.register_goal_value(goal_joint_positions) + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi/4)]), atol=0.001)) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) for percent in [0.5, 1]: - current_joint_positions = [0.2*percent, -np.pi/4*percent] - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions))) + current_joint_positions = goal_joint_positions * percent + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], atol=0.001)) - self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], - atol=0.001)) + self.assertTrue( + np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_positions[1], + atol=0.001)) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2)*(1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi/4)*(1-percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi / 4) * (1 - percent), places=5) def test_list_of_poses_goal(self): position_goal = [0.0, 1.0, 0.0] - orientation_goal = np.array([0, 0, np.pi/2]) + orientation_goal = np.array([0, 0, np.pi / 2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), lambda: [self.robot.get_pose(), self.robot.get_pose()]) - goal_validator.register_goal_value(poses_goal) + goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, np.pi/2, 1.0, np.pi/2]), atol=0.001)) + self.assertTrue( + np.allclose(goal_validator.current_error, np.array([1.0, np.pi / 2, 1.0, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: current_orientation_goal = orientation_goal * percent - current_pose_goal = Pose([0.0, 1.0*percent, 0.0], + current_pose_goal = Pose([0.0, 1.0 * percent, 0.0], quaternion_from_euler(*current_orientation_goal.tolist())) self.robot.set_pose(current_pose_goal) self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_pose_goal.position_as_list(), @@ -150,23 +149,23 @@ def test_list_of_poses_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1-percent) /2, places=5) - self.assertAlmostEqual(goal_validator.current_error[2], (1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1-percent) /2, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error[2], (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1 - percent) / 2, places=5) def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) - goal_validator.register_goal_value(positions_goal) + goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) for percent in [0.5, 1]: - current_position_goal = [0.0, 1.0*percent, 0.0] + current_position_goal = [0.0, 1.0 * percent, 0.0] self.robot.set_position(current_position_goal) self.assertTrue(np.allclose(self.robot.get_position_as_list(), current_position_goal, atol=0.001)) if percent == 1: @@ -174,20 +173,20 @@ def test_list_of_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1-percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 1-percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], 1 - percent, places=5) def test_list_of_orientations_goal(self): - orientation_goal = np.array([0, 0, np.pi/2]) + orientation_goal = np.array([0, 0, np.pi / 2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), - quaternion_from_euler(*orientation_goal.tolist())] + quaternion_from_euler(*orientation_goal.tolist())] goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), lambda: [self.robot.get_orientation_as_list(), self.robot.get_orientation_as_list()]) - goal_validator.register_goal_value(orientations_goals) + goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi/2, np.pi/2]), atol=0.001)) + self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi / 2, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: current_orientation_goal = orientation_goal * percent @@ -200,30 +199,30 @@ def test_list_of_orientations_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], np.pi*(1-percent)/2, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi*(1-percent)/2, places=5) + self.assertAlmostEqual(goal_validator.current_error[0], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) def test_list_of_revolute_joint_positions_goal(self): - goal_joint_position = -np.pi/4 - goal_joint_positions = [goal_joint_position, goal_joint_position] + goal_joint_position = -np.pi / 4 + goal_joint_positions = np.array([goal_joint_position, goal_joint_position]) + joint_names = ['l_shoulder_lift_joint', 'r_shoulder_lift_joint'] goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), - lambda: [self.robot.get_joint_position('l_shoulder_lift_joint'), - self.robot.get_joint_position('l_shoulder_lift_joint')]) - goal_validator.register_goal_value(goal_joint_positions) + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) for percent in [0.5, 1]: - current_joint_position = goal_joint_position * percent - self.robot.set_joint_position('l_shoulder_lift_joint', current_joint_position) - self.assertTrue(np.allclose(self.robot.get_joint_position('l_shoulder_lift_joint'), current_joint_position, atol=0.001)) + current_joint_position = goal_joint_positions * percent + self.robot.set_joint_positions(dict(zip(joint_names, current_joint_position))) + self.assertTrue(np.allclose(list(self.robot.get_multiple_joint_positions(joint_names).values()), + current_joint_position, atol=0.001)) if percent == 1: self.assertTrue(goal_validator.goal_achieved) else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position)*(1-percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position)*(1-percent), places=5) - + self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position) * (1 - percent), places=5) From 77a17579bfae0ffc8c4bb983cb99ad79546b0e69 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 16:54:59 +0200 Subject: [PATCH 069/189] [Multiverse] updated register goal value function to also take current_value_getter_input. --- test/test_goal_validator.py | 53 ++++++++++++++++++++----------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 5f8c6b1a2..a5be07e65 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -19,15 +19,15 @@ def test_single_pose_goal(self): goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) - self.assertAlmostEqual(goal_validator.current_error[0], 0.5, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0.5, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) print(goal_validator.current_error) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) - self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) def test_single_position_goal(self): cereal_goal_position = [1.3, 1.5, 0.95] @@ -54,13 +54,13 @@ def test_single_orientation_goal(self): self.assertAlmostEqual(v1, v2, places=5) self.assertTrue(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) - self.assertAlmostEqual(goal_validator.current_error[0], 0, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) def test_single_revolute_joint_position_goal(self): goal_joint_position = -np.pi / 4 - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), - lambda: self.robot.get_joint_position('l_shoulder_lift_joint')) - goal_validator.register_goal(goal_joint_position) + joint_name = 'l_shoulder_lift_joint' + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) + goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -73,14 +73,14 @@ def test_single_revolute_joint_position_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) def test_single_prismatic_joint_position_goal(self): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), - lambda: self.robot.get_joint_position(torso)) - goal_validator.register_goal(goal_joint_position) + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) + goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -93,7 +93,8 @@ def test_single_prismatic_joint_position_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) def test_multi_joint_goal(self): goal_joint_positions = np.array([0.2, -np.pi / 4]) @@ -119,8 +120,8 @@ def test_multi_joint_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(0.2) * (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(-np.pi / 4) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(0.2) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(-np.pi / 4) * (1 - percent), places=5) def test_list_of_poses_goal(self): position_goal = [0.0, 1.0, 0.0] @@ -149,10 +150,10 @@ def test_list_of_poses_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) - self.assertAlmostEqual(goal_validator.current_error[2], (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[3], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[2], (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[3], np.pi * (1 - percent) / 2, places=5) def test_list_of_positions_goal(self): position_goal = [0.0, 1.0, 0.0] @@ -173,8 +174,8 @@ def test_list_of_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], 1 - percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 1 - percent, places=5) def test_list_of_orientations_goal(self): orientation_goal = np.array([0, 0, np.pi / 2]) @@ -199,8 +200,8 @@ def test_list_of_orientations_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], np.pi * (1 - percent) / 2, places=5) - self.assertAlmostEqual(goal_validator.current_error[1], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], np.pi * (1 - percent) / 2, places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) def test_list_of_revolute_joint_positions_goal(self): goal_joint_position = -np.pi / 4 @@ -224,5 +225,7 @@ def test_list_of_revolute_joint_positions_goal(self): else: self.assertFalse(goal_validator.goal_achieved) self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, percent, places=5) - self.assertAlmostEqual(goal_validator.current_error[0], abs(goal_joint_position) * (1 - percent), places=5) - self.assertAlmostEqual(goal_validator.current_error[1], abs(goal_joint_position) * (1 - percent), places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), + places=5) + self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(goal_joint_position) * (1 - percent), + places=5) From 0724c89390b8e093bee8622a3af9e557b3b40358 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 23 Jul 2024 22:32:45 +0200 Subject: [PATCH 070/189] [Multiverse] tests are working, demo sometimes fails due to wierd behaviour of bowl object. --- src/pycram/datastructures/world.py | 153 ++++++++++- src/pycram/world_concepts/world_object.py | 48 +++- src/pycram/worlds/bullet_world.py | 37 ++- src/pycram/worlds/multiverse.py | 150 +++++------ .../multiverse_communication/clients.py | 34 +-- .../multiverse_functions/error_checkers.py | 52 ++-- .../multiverse_functions/goal_validator.py | 237 +++++++++++++++++- test/test_goal_validator.py | 136 ++++++++-- test/test_multiverse.py | 2 - 9 files changed, 689 insertions(+), 160 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 8051313e5..e6adba567 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -7,7 +7,6 @@ from abc import ABC, abstractmethod from copy import copy - import numpy as np import rospy from geometry_msgs.msg import Point @@ -28,6 +27,9 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event +from ..worlds.multiverse_functions.goal_validator import GoalValidator, MultiPoseGoalValidator, \ + MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ + OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -158,6 +160,13 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ + acceptable_position_error: float = 1e-2 + acceptable_orientation_error: float = 5*np.pi / 180 + acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) + """ + The acceptable error for the position and orientation of an object. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -204,6 +213,45 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._current_state: Optional[WorldState] = None + self._init_goal_validators() + + def _init_goal_validators(self): + """ + Initializes the goal validators for the World objects' poses, positions, and orientations. + """ + # Goal validators for multiple objects + self.multi_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error) + self.multi_position_goal_validator = MultiPositionGoalValidator( + lambda x: list(self.get_multiple_object_positions(x).values()), self.acceptable_position_error) + self.multi_orientation_goal_validator = MultiOrientationGoalValidator( + lambda x: list(self.get_multiple_object_orientations(x).values()), self.acceptable_orientation_error) + + # Goal validators for an object + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error) + self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error) + self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, + self.acceptable_orientation_error) + + # Goal validators for the links of an object + self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) + self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, + self.acceptable_position_error) + self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, + self.acceptable_orientation_error) + + self.multi_link_pose_goal_validator = MultiPoseGoalValidator( + lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error) + self.multi_link_position_goal_validator = MultiPositionGoalValidator( + lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error) + self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( + lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error) + + # Goal validators for the joints of an object + self.joint_position_goal_validator = JointPositionGoalValidator(self.get_joint_position) + self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.get_multiple_joint_positions(x).values())) + @abstractmethod def _init_world(self, mode: WorldMode): """ @@ -440,6 +488,56 @@ def get_link_pose(self, link: Link) -> Pose: """ pass + @abstractmethod + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and Pose objects as values. + """ + pass + + @abstractmethod + def get_link_position(self, link: Link) -> List[float]: + """ + Get the position of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The position of the link as a list of floats. + """ + pass + + @abstractmethod + def get_link_orientation(self, link: Link) -> List[float]: + """ + Get the orientation of a link of an articulated object with respect to the world frame. + + :param link: The link as a AbstractLink object. + :return: The orientation of the link as a list of floats. + """ + pass + + @abstractmethod + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + + @abstractmethod + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of an articulated object with respect to the world frame. + + :param links: The links as a list of AbstractLink objects. + :return: A dictionary with link names as keys and lists of floats as values. + """ + pass + @abstractmethod def get_object_link_names(self, obj: Object) -> List[str]: """ @@ -489,6 +587,41 @@ def get_object_pose(self, obj: Object) -> Pose: """ pass + @abstractmethod + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + """ + Get the poses of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the positions of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_object_position(self, obj: Object) -> List[float]: + """ + Get the position of an object in the world frame from the current object pose in the simulator. + """ + pass + + @abstractmethod + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple objects in the world frame from the current object poses in the simulator. + """ + pass + + @abstractmethod + def get_object_orientation(self, obj: Object) -> List[float]: + """ + Get the orientation of an object in the world frame from the current object pose in the simulator. + """ + pass + def set_mobile_robot_pose(self, pose: Pose) -> None: """ Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when @@ -496,9 +629,9 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal) + self.robot.set_joint_positions(goal, validate=False) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[Joint, float]: + def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. param pose: The target pose. @@ -607,10 +740,12 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: pass @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: """ Set the positions of multiple joints of an articulated object. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + :param validate: If the joint position goals should be validated. """ pass @@ -632,6 +767,16 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): """ pass + @abstractmethod + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + """ + Reset the world position and orientation of the base of multiple objects instantaneously, + not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. + + :param objects: A dictionary with objects as keys and poses as values. + """ + pass + @abstractmethod def step(self): """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d08918b56..11da4ce10 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,17 +8,22 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint, JointDescription -from ..object_descriptors.urdf import ObjectDescription as URDFObject -from ..datastructures.world import WorldEntity, World -from ..world_concepts.constraints import Attachment from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, ContactPointsList) from ..datastructures.enums import ObjectType, JointType -from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose, Transform +from ..datastructures.world import WorldEntity, World +from ..description import ObjectDescription, LinkDescription, Joint +from ..local_transformer import LocalTransformer +from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription +from ..world_concepts.constraints import Attachment +from ..worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, IterableErrorChecker +from ..worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, OrientationGoalValidator, \ + PositionGoalValidator, MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator, \ + MultiJointPositionGoalValidator, JointPositionGoalValidator Link = ObjectDescription.Link @@ -63,6 +68,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if name in [obj.name for obj in self.world.objects]: rospy.logerr(f"An object with the name {name} already exists in the world.") raise ValueError(f"An object with the name {name} already exists in the world.") + self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color @@ -92,6 +98,30 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the positions of multiple links of the object. + param link_names: The names of the links. + return: The positions of the links. + """ + return self.world.get_multiple_link_positions(links) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + """ + Get the orientations of multiple links of the object. + param link_names: The names of the links. + return: The orientations of the links. + """ + return self.world.get_multiple_link_orientations(links) + + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + """ + Get the poses of multiple links of the object. + param link_names: The names of the links. + return: The poses of the links. + """ + return self.world.get_multiple_link_poses(links) + def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the target poses of the attached objects. @@ -614,7 +644,8 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) + return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), + self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -875,16 +906,17 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float], validate: Optional[bool] = True) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_positions: A dictionary with the joint names as keys and the target positions as values. + :param validate: If True the joint position goals will be validated. """ joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} - self.world.set_multiple_joint_positions(joint_positions) + self.world.set_multiple_joint_positions(joint_positions, validate) self.update_pose() self._update_all_links_poses() self.update_link_transforms() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 8be0deaec..e809aff2a 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -113,6 +113,21 @@ def get_object_joint_names(self, obj: Object) -> List[str]: return [p.getJointInfo(obj.id, i, physicsClientId=self.id)[1].decode('utf-8') for i in range(self.get_object_number_of_joints(obj))] + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return {link.name: self.get_link_pose(link) for link in links} + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_position(link) for link in links} + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return {link.name: self.get_link_orientation(link) for link in links} + + def get_link_position(self, link: Link) -> List[float]: + return self.get_link_pose(link).position_as_list() + + def get_link_orientation(self, link: Link) -> List[float]: + return self.get_link_pose(link).orientation_as_list() + def get_link_pose(self, link: ObjectDescription.Link) -> Pose: bullet_link_state = p.getLinkState(link.object_id, link.id, physicsClientId=self.id) return Pose(*bullet_link_state[4:6]) @@ -167,7 +182,8 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) @@ -181,9 +197,28 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + for obj, pose in objects.items(): + self.reset_object_base_pose(obj, pose) + def step(self): p.stepSimulation(physicsClientId=self.id) + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return {obj.name: self.get_object_pose(obj) for obj in objects} + + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).position_as_list() for obj in objects} + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return {obj.name: self.get_object_pose(obj).orientation_as_list() for obj in objects} + + def get_object_position(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).position_as_list() + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.get_object_pose(obj).orientation_as_list() + def get_object_pose(self, obj: Object) -> Pose: return Pose(*p.getBasePositionAndOrientation(obj.id, physicsClientId=self.id)) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b4aabd858..414b2e024 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,9 +7,7 @@ from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager -from .multiverse_functions.error_checkers import PoseErrorChecker, MultiJointPositionErrorChecker, \ - RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker -from .multiverse_functions.goal_validator import GoalValidator + from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -182,37 +180,42 @@ def get_object_joint_names(self, obj: Object) -> List[str]: def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] + def get_link_position(self, link: Link) -> List[float]: + return self.reader.get_body_position(link.name) + + def get_link_orientation(self, link: Link) -> List[float]: + return self.reader.get_body_orientation(link.name) + + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([link.name for link in links]) + + def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([link.name for link in links]) + def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - self.set_multiple_joint_positions({joint: joint_position}) + self.joint_position_goal_validator.register_goal(joint_position, joint.type, joint) + self.writer.send_body_data_to_server(joint.name, + {self.get_joint_position_name(joint): [joint_position]}) + self.joint_position_goal_validator.wait_until_goal_is_achieved() - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> None: - initial_joint_positions = {joint: self.get_joint_position(joint) for joint in joint_positions.keys()} + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], + validate: Optional[bool] = True) -> None: data = {joint.name: {self.get_joint_position_name(joint): [position]} for joint, position in joint_positions.items()} + + if validate: + self.multi_joint_position_goal_validator.register_goal(list(joint_positions.values()), + [joint.type for joint in joint_positions.keys()], + list(joint_positions.keys())) self.writer.send_multiple_body_data_to_server(data) - self._wait_until_multiple_joint_goals_are_achieved(joint_positions, initial_joint_positions) - - def _wait_until_multiple_joint_goals_are_achieved(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> None: - goal_validator = self._get_multi_joint_goal_validator(joint_positions, initial_joint_positions) - self._wait_until_goal_is_achieved(goal_validator) - - def _get_multi_joint_goal_validator(self, joint_positions: Dict[Joint, float], - initial_joint_positions: Dict[Joint, float]) -> GoalValidator: - joints = list(joint_positions.keys()) - joint_types = [joint.type for joint in joints] - target_joint_positions = list(joint_positions.values()) - initial_joint_positions = list(initial_joint_positions.values()) - goal_validator = GoalValidator(target_joint_positions, - lambda: list(self.get_multiple_joint_positions(joints).values()), - MultiJointPositionErrorChecker(joint_types), - initial_value=initial_joint_positions) - return goal_validator + + if validate: + self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() def get_joint_position(self, joint: Joint) -> float: - data = self.get_multiple_joint_positions([joint]) + data = self.reader.get_body_data(joint.name, [self.get_joint_position_name(joint)]) if data is not None: - return data[joint.name] + return data[self.get_joint_position_name(joint)][0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: self.check_object_exists_and_issue_warning_if_not(joints[0].object) @@ -222,36 +225,45 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str if data is not None: return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} - def _wait_until_joint_goal_is_achieved(self, joint: Joint, joint_position: float, - initial_joint_position: float) -> None: - error_checker = RevoluteJointPositionErrorChecker() if joint.type == JointType.REVOLUTE else \ - PrismaticJointPositionErrorChecker() - goal_validator = GoalValidator(joint_position, lambda: self.get_joint_position(joint), - error_checker, initial_value=initial_joint_position) - self._wait_until_goal_is_achieved(goal_validator) - def get_link_pose(self, link: Link) -> Pose: self.check_object_exists_and_issue_warning_if_not(link.object) return self._get_body_pose(link.name) + def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([link.name for link in links]) + def get_object_pose(self, obj: Object) -> Pose: self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return Pose() return self._get_body_pose(obj.name) + def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: + return self._get_multiple_body_poses([obj.name for obj in objects]) + def reset_object_base_pose(self, obj: Object, pose: Pose): self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return + + self.pose_goal_validator.register_goal(pose, obj) + initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) + if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): self.set_mobile_robot_pose(pose) else: - initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) self._set_body_pose(obj.name, pose) - if len(initial_attached_objects_poses) > 0: - self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + self.pose_goal_validator.wait_until_goal_is_achieved() + if len(initial_attached_objects_poses) > 0: + self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, + # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. + for obj, pose in objects.items(): + self.reset_object_base_pose(obj, pose) def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: """ @@ -260,20 +272,9 @@ def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_po param initial_poses: The list of initial poses of the attached objects. """ target_poses = obj.get_target_poses_of_attached_objects() - body_names = [obj.name for obj in target_poses.keys()] - self._wait_until_all_pose_goals_are_achieved(body_names, list(target_poses.values()), - initial_poses) - - def _wait_until_all_pose_goals_are_achieved(self, body_names: List[str], poses: List[Pose], - initial_poses: List[Pose]) -> None: - """ - Wait until all poses are set to the desired poses. - param poses: The dictionary of the desired poses - param initial_poses: The dictionary of the initial poses - """ - goal_validator = GoalValidator(poses, lambda: list(self._get_multiple_body_poses(body_names).values()), - PoseErrorChecker(is_iterable=True), initial_value=initial_poses) - self._wait_until_goal_is_achieved(goal_validator) + self.multi_pose_goal_validator.register_goal_and_wait_until_achieved(list(target_poses.values()), + list(target_poses.keys()), + initial_poses) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ @@ -284,12 +285,21 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona """ return self.reader.get_body_pose(body_name, wait) - def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: - return self._get_multiple_body_poses([obj.name for obj in objects]) - def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: return self.reader.get_multiple_body_poses(body_names) + def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_positions([obj.name for obj in objects]) + + def get_object_position(self, obj: Object) -> List[float]: + return self.reader.get_body_position(obj.name) + + def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: + return self.reader.get_multiple_body_orientations([obj.name for obj in objects]) + + def get_object_orientation(self, obj: Object) -> List[float]: + return self.reader.get_body_orientation(obj.name) + def multiverse_reset_world(self): self.writer.reset_world() @@ -301,39 +311,7 @@ def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ xyzw = pose.orientation_as_list() wxyz = [xyzw[3], *xyzw[:3]] - initial_pose = self._get_body_pose(body_name, wait=False) self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) - if initial_pose is not None: - self._wait_until_pose_goal_is_achieved(body_name, pose, initial_pose) - - def _wait_until_pose_goal_is_achieved(self, body_name: str, target_pose: Pose, initial_pose: Pose): - """ - Wait until the pose of a body is set. - param body_name: The name of the body. - param target_pose: The target pose of the body. - param initial_pose: The initial pose of the body. - """ - goal_validator = GoalValidator(PoseErrorChecker(), lambda: self._get_body_pose(body_name)) - goal_validator.register_goal(target_pose, initial_pose) - self._wait_until_goal_is_achieved(goal_validator) - - def _wait_until_goal_is_achieved(self, goal_validator: GoalValidator) -> None: - """ - Wait until the target is reached. - param goal_validator: The goal validator object to validate and keep track of the goal achievement progress. - """ - start_time = time() - current = goal_validator.current_value - while not goal_validator.goal_achieved: - sleep(0.01) - if time() - start_time > self.reader.MAX_WAIT_TIME_FOR_DATA: - msg = f"Failed to achieve goal from initial error {goal_validator.initial_error} with" \ - f" goal {goal_validator.goal_value} within {self.reader.MAX_WAIT_TIME_FOR_DATA}" \ - f" seconds, the current value is {current}, error is {goal_validator.current_error}, percentage" \ - f" of goal achieved is {goal_validator.percentage_of_goal_achieved}" - logging.error(msg) - raise TimeoutError(msg) - current = goal_validator.current_value def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 6e3f9f113..c6cfecfb2 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -59,7 +59,7 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = self.thread.start() - def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose]: + def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Pose]: """ Get the body pose from the multiverse server. param name: The name of the body. @@ -70,7 +70,7 @@ def get_body_pose(self, name: str, wait: Optional[bool] = True) -> Optional[Pose if data is not None: return data[name] - def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = True) -> Optional[Dict[str, Pose]]: + def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, Pose]]: """ Get the body poses from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -83,7 +83,7 @@ def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) for name in body_names} - def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body position from the multiverse server. param name: The name of the body. @@ -93,7 +93,7 @@ def get_body_position(self, name: str, wait: Optional[bool] = True) -> Optional[ return self.get_multiple_body_positions([name], wait=wait)[name] def get_multiple_body_positions(self, body_names: List[str], - wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body positions from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -102,7 +102,7 @@ def get_multiple_body_positions(self, body_names: List[str], """ return self.get_multiple_body_properties(body_names, ["position"], wait=wait) - def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body orientation from the multiverse server. param name: The name of the body. @@ -112,7 +112,7 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = True) -> Option return self.get_multiple_body_orientations([name], wait=wait)[name] def get_multiple_body_orientations(self, body_names: List[str], - wait: Optional[bool] = True) -> Optional[Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body orientations from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -123,7 +123,7 @@ def get_multiple_body_orientations(self, body_names: List[str], if data is not None: return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} - def get_body_property(self, name: str, property_name: str, wait: Optional[bool] = True) -> Optional[List[float]]: + def get_body_property(self, name: str, property_name: str) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. @@ -131,12 +131,12 @@ def get_body_property(self, name: str, property_name: str, wait: Optional[bool] param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_name], wait=wait) - if data is not None: + data = self.get_received_data() + if self.check_for_body_data(name, data, [property_name]): return data[name][property_name] def get_multiple_body_properties(self, body_names: List[str], properties: List[str], - wait: Optional[bool] = True) -> Dict[str, Dict[str, List[float]]]: + wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: """ Get the body properties from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -156,8 +156,7 @@ def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: return quaternion[1:] + [quaternion[0]] def get_body_data(self, name: str, - properties: Optional[List[str]] = None, - wait: Optional[bool] = True) -> Optional[Dict]: + properties: Optional[List[str]] = None) -> Optional[Dict]: """ Get the body data from the multiverse server. param name: The name of the body. @@ -165,11 +164,12 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - properties = {name: properties} if properties is not None else None - return self.get_multiple_body_data([name], properties, wait=wait)[name] + data = self.get_received_data() + if self.check_for_body_data(name, data, properties): + return data[name] def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, - wait: Optional[bool] = True) -> Dict: + wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -347,7 +347,9 @@ def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) param data: The data to be sent. return: The response from the server. """ - return self.send_multiple_body_data_to_server({body_name: data}) + flattened_data = [value for values in data.values() for value in values] + return self.send_data_to_server([time() - self.time_start, *flattened_data], + send_meta_data={body_name: list(data.keys())}) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[float]]]) -> Dict: """ diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index cf85425d9..2b49a5787 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -20,17 +20,25 @@ def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterabl :param acceptable_error: The acceptable error. :param is_iterable: Whether the error is iterable (i.e. list of errors). """ - self.acceptable_error: np.ndarray = np.array(acceptable_error) + self._acceptable_error: np.ndarray = np.array(acceptable_error) self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable + @property + def acceptable_error(self) -> np.ndarray: + return self._acceptable_error + + @acceptable_error.setter + def acceptable_error(self, new_acceptable_error: Union[float, T_Iterable[float]]) -> None: + self._acceptable_error = np.array(new_acceptable_error) + def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[float]] = None, tile_to_match: Optional[Sized] = None,) -> None: """ Update the acceptable error with a new value, and tile it to match the length of the error if needed. """ if new_acceptable_error is not None: - self.acceptable_error = np.array(new_acceptable_error) + self.acceptable_error = new_acceptable_error if tile_to_match is not None and self.is_iterable: self.update_tiled_acceptable_error(tile_to_match) @@ -120,37 +128,49 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: return calculate_orientation_error(value_1, value_2) -class MultiJointPositionErrorChecker(ErrorChecker): +class SingleValueErrorChecker(ErrorChecker): - def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): - self.joint_types = joint_types - if acceptable_error is None: - acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] - super().__init__(acceptable_error, True) + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ - Calculate the error between two joint positions. + Calculate the error between two values. """ - return calculate_joint_position_error(value_1, value_2) + return abs(value_1 - value_2) -class RevoluteJointPositionErrorChecker(ErrorChecker): +class RevoluteJointPositionErrorChecker(SingleValueErrorChecker): def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): super().__init__(acceptable_error, is_iterable) + +class PrismaticJointPositionErrorChecker(SingleValueErrorChecker): + + def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + super().__init__(acceptable_error, is_iterable) + + +class IterableErrorChecker(ErrorChecker): + + def __init__(self, acceptable_error: Optional[T_Iterable[float]] = None): + super().__init__(acceptable_error, True) + def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ - Calculate the error between two joint positions. + Calculate the error between two values. """ - return calculate_joint_position_error(value_1, value_2) + return abs(value_1 - value_2) -class PrismaticJointPositionErrorChecker(ErrorChecker): +class MultiJointPositionErrorChecker(IterableErrorChecker): - def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): - super().__init__(acceptable_error, is_iterable) + def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): + self.joint_types = joint_types + if acceptable_error is None: + acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] + super().__init__(acceptable_error) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index b6fedf2fb..423a8a9cd 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,7 +1,14 @@ +import logging +from time import sleep, time + import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, Protocol +from typing_extensions import Any, Callable, Optional, Union, Iterable + +from pycram.datastructures.enums import JointType +from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ + OrientationErrorChecker, SingleValueErrorChecker -from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker +OptionalArgCallable = Union[Callable[[], Any], Callable[[Any], Any]] class GoalValidator: @@ -9,7 +16,7 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ - def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[Optional[Any]], Any], + def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalArgCallable, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. @@ -26,6 +33,44 @@ def __init__(self, error_checker: ErrorChecker, current_value_getter: Callable[[ self.initial_error: Optional[np.ndarray] = None self.current_value_getter_input: Optional[Any] = None + def register_goal_and_wait_until_achieved(self, goal_value: Any, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Union[float, Iterable[float]]] = None, + max_wait_time: Optional[float] = 2, + time_per_read: Optional[float] = 0.01) -> None: + """ + Register the goal value and wait until the target is reached. + :param goal_value: The goal value. + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + self.wait_until_goal_is_achieved(max_wait_time, time_per_read) + + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, + time_per_read: Optional[float] = 0.01) -> None: + """ + Wait until the target is reached. + :param max_wait_time: The maximum time to wait. + :param time_per_read: The time to wait between each read. + """ + start_time = time() + current = self.current_value + while not self.goal_achieved: + sleep(time_per_read) + if time() - start_time > max_wait_time: + msg = f"Failed to achieve goal from initial error {self.initial_error} with" \ + f" goal {self.goal_value} within {max_wait_time}" \ + f" seconds, the current value is {current}, error is {self.current_error}, percentage" \ + f" of goal achieved is {self.percentage_of_goal_achieved}" + logging.error(msg) + raise TimeoutError(msg) + current = self.current_value + @property def _acceptable_error(self) -> np.ndarray: """ @@ -143,7 +188,7 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n :param error: The error. :param threshold: The threshold. """ - return np.maximum(error-self._acceptable_error, threshold) + return np.maximum(error - self._acceptable_error, threshold) @property def goal_achieved(self) -> bool: @@ -163,3 +208,187 @@ def is_current_error_acceptable(self) -> bool: return: Whether the error is acceptable. """ return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) + + +class PoseGoalValidator(GoalValidator): + """ + A class to validate the pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_pose_getter: OptionalArgCallable = None, + acceptable_error: Union[float, Iterable[float]] = (1e-3, np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the pose goal validator. + :param current_pose_getter: The current pose getter function which takes an optional input and returns the + current pose. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(PoseErrorChecker(acceptable_error, is_iterable=is_iterable), current_pose_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPoseGoalValidator(PoseGoalValidator): + """ + A class to validate the multi-pose goal by tracking the goal achievement progress. + """ + + def __init__(self, current_poses_getter: OptionalArgCallable = None, + acceptable_error: Union[float, Iterable[float]] = (1e-2, 5*np.pi / 180), + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-pose goal validator. + :param current_poses_getter: The current poses getter function which takes an optional input and returns the + current poses. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_poses_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class PositionGoalValidator(GoalValidator): + """ + A class to validate the position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and + returns the current position. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of position vectors. + """ + super().__init__(PositionErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiPositionGoalValidator(PositionGoalValidator): + """ + A class to validate the multi-position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = 1e-3, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_positions_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class OrientationGoalValidator(GoalValidator): + """ + A class to validate the orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientation_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the orientation goal validator. + :param current_orientation_getter: The current orientation getter function which takes an optional input and + returns the current orientation. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of quaternions. + """ + super().__init__(OrientationErrorChecker(acceptable_error, is_iterable=is_iterable), current_orientation_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + +class MultiOrientationGoalValidator(OrientationGoalValidator): + """ + A class to validate the multi-orientation goal by tracking the goal achievement progress. + """ + + def __init__(self, current_orientations_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = np.pi / 180, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-orientation goal validator. + :param current_orientations_getter: The current orientations getter function which takes an optional input and + returns the current orientations. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(current_orientations_getter, acceptable_error, acceptable_percentage_of_goal_achieved, + is_iterable=True) + + +class JointPositionGoalValidator(GoalValidator): + """ + A class to validate the joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_position_getter: OptionalArgCallable = None, + acceptable_error: Optional[float] = None, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, + is_iterable: Optional[bool] = False): + """ + Initialize the joint position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and returns + the current position. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + :param is_iterable: Whether it is a sequence of joint positions. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, + acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + + def register_goal(self, goal_value: Any, joint_type: JointType, + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[float] = None): + """ + Register the goal value. + :param goal_value: The goal value. + :param joint_type: The joint type (e.g. REVOLUTE, PRISMATIC). + :param current_value_getter_input: The values that are used as input to the current value getter. + :param initial_value: The initial value. + :param acceptable_error: The acceptable error. + """ + if acceptable_error is None: + self.error_checker.acceptable_error = np.pi / 180 if joint_type == JointType.REVOLUTE else 1e-3 + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +class MultiJointPositionGoalValidator(GoalValidator): + """ + A class to validate the multi-joint position goal by tracking the goal achievement progress. + """ + + def __init__(self, current_positions_getter: OptionalArgCallable = None, + acceptable_error: Optional[Iterable[float]] = None, + acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + """ + Initialize the multi-joint position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and + returns the current positions. + :param acceptable_error: The acceptable error. + :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. + """ + super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, + acceptable_percentage_of_goal_achieved) + + def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], + current_value_getter_input: Optional[Any] = None, + initial_value: Optional[Any] = None, + acceptable_error: Optional[Iterable[float]] = None): + if acceptable_error is None: + self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in + joint_type] + super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index a5be07e65..eed9e8203 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -1,5 +1,6 @@ import numpy as np from tf.transformations import quaternion_from_euler +from typing_extensions import Optional, List from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import JointType @@ -8,14 +9,23 @@ from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator +from pycram.worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ + PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ + MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator class TestGoalValidator(BulletWorldTestCase): def test_single_pose_goal(self): + pose_goal_validators = PoseGoalValidator(self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def test_single_pose_goal_generic(self): + pose_goal_validators = GoalValidator(PoseErrorChecker(), self.milk.get_pose) + self.validate_pose_goal(pose_goal_validators) + + def validate_pose_goal(self, goal_validator): milk_goal_pose = Pose([1.3, 1.5, 0.9]) - goal_validator = GoalValidator(PoseErrorChecker(), self.milk.get_pose) goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -24,14 +34,20 @@ def test_single_pose_goal(self): self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) - print(goal_validator.current_error) self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) + def test_single_position_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + def test_single_position_goal(self): + goal_validator = PositionGoalValidator(self.cereal.get_position_as_list) + self.validate_position_goal(goal_validator) + + def validate_position_goal(self, goal_validator): cereal_goal_position = [1.3, 1.5, 0.95] - goal_validator = GoalValidator(PositionErrorChecker(), self.cereal.get_position_as_list) goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -42,9 +58,16 @@ def test_single_position_goal(self): self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertEqual(goal_validator.current_error, 0) + def test_single_orientation_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + def test_single_orientation_goal(self): + goal_validator = OrientationGoalValidator(self.cereal.get_orientation_as_list) + self.validate_orientation_goal(goal_validator) + + def validate_orientation_goal(self, goal_validator): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) - goal_validator = GoalValidator(OrientationErrorChecker(), self.cereal.get_orientation_as_list) goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -56,11 +79,21 @@ def test_single_orientation_goal(self): self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) + def test_single_revolute_joint_position_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator) + def test_single_revolute_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_revolute_joint_position_goal(goal_validator, JointType.REVOLUTE) + + def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): goal_joint_position = -np.pi / 4 joint_name = 'l_shoulder_lift_joint' - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(), self.robot.get_joint_position) - goal_validator.register_goal(goal_joint_position, joint_name) + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, joint_name) + else: + goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -76,11 +109,21 @@ def test_single_revolute_joint_position_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), places=5) + def test_single_prismatic_joint_position_goal_generic(self): + goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator) + def test_single_prismatic_joint_position_goal(self): + goal_validator = JointPositionGoalValidator(self.robot.get_joint_position) + self.validate_prismatic_joint_position_goal(goal_validator, JointType.PRISMATIC) + + def validate_prismatic_joint_position_goal(self, goal_validator, joint_type: Optional[JointType] = None): goal_joint_position = 0.2 torso = RobotDescription.current_robot_description.torso_joint - goal_validator = GoalValidator(PrismaticJointPositionErrorChecker(), self.robot.get_joint_position) - goal_validator.register_goal(goal_joint_position, torso) + if joint_type is not None: + goal_validator.register_goal(goal_joint_position, joint_type, torso) + else: + goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) @@ -96,13 +139,25 @@ def test_single_prismatic_joint_position_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(goal_joint_position) * (1 - percent), places=5) - def test_multi_joint_goal(self): - goal_joint_positions = np.array([0.2, -np.pi / 4]) - joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + def test_multi_joint_goal_generic(self): joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] goal_validator = GoalValidator(MultiJointPositionErrorChecker(joint_types), lambda x: list(self.robot.get_multiple_joint_positions(x).values())) - goal_validator.register_goal(goal_joint_positions, joint_names) + self.validate_multi_joint_goal(goal_validator) + + def test_multi_joint_goal(self): + joint_types = [JointType.PRISMATIC, JointType.REVOLUTE] + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_multi_joint_goal(goal_validator, joint_types) + + def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[JointType]] = None): + goal_joint_positions = np.array([0.2, -np.pi / 4]) + joint_names = ['torso_lift_joint', 'l_shoulder_lift_joint'] + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) @@ -123,13 +178,20 @@ def test_multi_joint_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], abs(0.2) * (1 - percent), places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], abs(-np.pi / 4) * (1 - percent), places=5) + def test_list_of_poses_goal_generic(self): + goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), + lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + def test_list_of_poses_goal(self): + goal_validator = MultiPoseGoalValidator(lambda: [self.robot.get_pose(), self.robot.get_pose()]) + self.validate_list_of_poses_goal(goal_validator) + + def validate_list_of_poses_goal(self, goal_validator): position_goal = [0.0, 1.0, 0.0] orientation_goal = np.array([0, 0, np.pi / 2]) poses_goal = [Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist())), Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] - goal_validator = GoalValidator(PoseErrorChecker(is_iterable=True), - lambda: [self.robot.get_pose(), self.robot.get_pose()]) goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -155,11 +217,19 @@ def test_list_of_poses_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[2], (1 - percent), places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[3], np.pi * (1 - percent) / 2, places=5) + def test_list_of_positions_goal_generic(self): + goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), + lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + def test_list_of_positions_goal(self): + goal_validator = MultiPositionGoalValidator(lambda: [self.robot.get_position_as_list(), + self.robot.get_position_as_list()]) + self.validate_list_of_positions_goal(goal_validator) + + def validate_list_of_positions_goal(self, goal_validator): position_goal = [0.0, 1.0, 0.0] positions_goal = [position_goal, position_goal] - goal_validator = GoalValidator(PositionErrorChecker(is_iterable=True), - lambda: [self.robot.get_position_as_list(), self.robot.get_position_as_list()]) goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -177,13 +247,21 @@ def test_list_of_positions_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 1 - percent, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 1 - percent, places=5) + def test_list_of_orientations_goal_generic(self): + goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), + lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + def test_list_of_orientations_goal(self): + goal_validator = MultiOrientationGoalValidator(lambda: [self.robot.get_orientation_as_list(), + self.robot.get_orientation_as_list()]) + self.validate_list_of_orientations_goal(goal_validator) + + def validate_list_of_orientations_goal(self, goal_validator): orientation_goal = np.array([0, 0, np.pi / 2]) orientations_goals = [quaternion_from_euler(*orientation_goal.tolist()), quaternion_from_euler(*orientation_goal.tolist())] - goal_validator = GoalValidator(OrientationErrorChecker(is_iterable=True), - lambda: [self.robot.get_orientation_as_list(), - self.robot.get_orientation_as_list()]) goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) @@ -203,13 +281,25 @@ def test_list_of_orientations_goal(self): self.assertAlmostEqual(goal_validator.current_error.tolist()[0], np.pi * (1 - percent) / 2, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], np.pi * (1 - percent) / 2, places=5) + def test_list_of_revolute_joint_positions_goal_generic(self): + goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator) + def test_list_of_revolute_joint_positions_goal(self): + goal_validator = MultiJointPositionGoalValidator( + lambda x: list(self.robot.get_multiple_joint_positions(x).values())) + self.validate_list_of_revolute_joint_positions_goal(goal_validator, [JointType.REVOLUTE, JointType.REVOLUTE]) + + def validate_list_of_revolute_joint_positions_goal(self, goal_validator, + joint_types: Optional[List[JointType]] = None): goal_joint_position = -np.pi / 4 goal_joint_positions = np.array([goal_joint_position, goal_joint_position]) joint_names = ['l_shoulder_lift_joint', 'r_shoulder_lift_joint'] - goal_validator = GoalValidator(RevoluteJointPositionErrorChecker(is_iterable=True), - lambda x: list(self.robot.get_multiple_joint_positions(x).values())) - goal_validator.register_goal(goal_joint_positions, joint_names) + if joint_types is not None: + goal_validator.register_goal(goal_joint_positions, joint_types, joint_names) + else: + goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, diff --git a/test/test_multiverse.py b/test/test_multiverse.py index e2af322f4..54faf4203 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -49,9 +49,7 @@ def tearDownClass(cls): cls.multiverse.exit() def tearDown(self): - # self.multiverse.multiverse_reset_world() self.multiverse.reset_world_and_remove_objects() - # MultiversePyCRAMTestCase.big_bowl = self.spawn_big_bowl() def test_demo(self): extension = ObjectDescription.get_file_extension() From b82a84f3d1d6b46a6352feeef0ccaf5795b8c1d0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 13:52:06 +0200 Subject: [PATCH 071/189] [Multiverse] tests are working. --- src/pycram/datastructures/world.py | 42 ++++++++++++------- src/pycram/world_concepts/world_object.py | 7 ++++ src/pycram/worlds/multiverse.py | 25 ++++++++--- .../multiverse_communication/clients.py | 9 ++-- .../multiverse_functions/goal_validator.py | 2 +- test/test_multiverse.py | 8 ++-- 6 files changed, 65 insertions(+), 28 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e6adba567..0f6a37501 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -160,9 +160,11 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ - acceptable_position_error: float = 1e-2 - acceptable_orientation_error: float = 5*np.pi / 180 + acceptable_position_error: float = 5e-3 # 5 cm + acceptable_orientation_error: float = 10 * np.pi / 180 # 5 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) + use_percentage_of_goal: bool = True + acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None """ The acceptable error for the position and orientation of an object. """ @@ -221,36 +223,48 @@ def _init_goal_validators(self): """ # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error) + lambda x: list(self.get_multiple_object_poses(x).values()), + self.acceptable_pose_error, self.acceptable_percentage_of_goal) self.multi_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_object_positions(x).values()), self.acceptable_position_error) + lambda x: list(self.get_multiple_object_positions(x).values()), + self.acceptable_position_error, self.acceptable_percentage_of_goal) self.multi_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_object_orientations(x).values()), self.acceptable_orientation_error) + lambda x: list(self.get_multiple_object_orientations(x).values()), + self.acceptable_orientation_error, self.acceptable_percentage_of_goal) # Goal validators for an object - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error) - self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error) + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, + self.acceptable_percentage_of_goal) + self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error, + self.acceptable_percentage_of_goal) self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, - self.acceptable_orientation_error) + self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) # Goal validators for the links of an object self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, self.acceptable_position_error) self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, - self.acceptable_orientation_error) + self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) self.multi_link_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error) + lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error, + self.acceptable_percentage_of_goal) self.multi_link_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error) + lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error, + self.acceptable_percentage_of_goal) self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error) + lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error, + self.acceptable_percentage_of_goal) # Goal validators for the joints of an object - self.joint_position_goal_validator = JointPositionGoalValidator(self.get_joint_position) + self.joint_position_goal_validator = JointPositionGoalValidator( + self.get_joint_position, acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( - lambda x: list(self.get_multiple_joint_positions(x).values())) + lambda x: list(self.get_multiple_joint_positions(x).values()), + acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) @abstractmethod def _init_world(self, mode: WorldMode): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 11da4ce10..bb313e735 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -458,6 +458,13 @@ def reset(self, remove_saved_states=True) -> None: if remove_saved_states: self.remove_saved_states() + def has_type_environment(self) -> bool: + """ + Check if the object is of type environment. + :return: True if the object is of type environment, False otherwise. + """ + return self.obj_type == ObjectType.ENVIRONMENT + def attach(self, child_object: Object, parent_link: Optional[str] = None, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 414b2e024..ed950d632 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -212,10 +212,10 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], if validate: self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() - def get_joint_position(self, joint: Joint) -> float: - data = self.reader.get_body_data(joint.name, [self.get_joint_position_name(joint)]) - if data is not None: - return data[self.get_joint_position_name(joint)][0] + def get_joint_position(self, joint: Joint) -> Optional[float]: + data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) + if data is not None and len(data) > 0: + return data[0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: self.check_object_exists_and_issue_warning_if_not(joints[0].object) @@ -242,8 +242,10 @@ def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: return self._get_multiple_body_poses([obj.name for obj in objects]) def reset_object_base_pose(self, obj: Object, pose: Pose): + self.check_object_exists_and_issue_warning_if_not(obj) - if obj.obj_type == ObjectType.ENVIRONMENT: + + if obj.has_type_environment(): return self.pose_goal_validator.register_goal(pose, obj) @@ -259,6 +261,19 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): if len(initial_attached_objects_poses) > 0: self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: + """ + Check if the object is a child in a fixed joint constraint. This means that the object is not free to move. + It should be moved according to the parent object. + :param obj: The object to check. + :return: True if the object is a child in a fixed joint constraint, False otherwise. + """ + constraints = list(self.constraints.values()) + for c in constraints: + if c.child_link.object == obj and c.type == JointType.FIXED: + return True + return False + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index c6cfecfb2..718fd3ad3 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -131,8 +131,8 @@ def get_body_property(self, name: str, property_name: str) -> Optional[List[floa param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_received_data() - if self.check_for_body_data(name, data, [property_name]): + data = self.get_multiple_body_properties([name], [property_name]) + if data is not None: return data[name][property_name] def get_multiple_body_properties(self, body_names: List[str], properties: List[str], @@ -164,8 +164,9 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - data = self.get_received_data() - if self.check_for_body_data(name, data, properties): + properties = {name: properties} if properties is not None else None + data = self.get_multiple_body_data([name], properties) + if data is not None: return data[name] def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 423a8a9cd..d7e43adf0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -179,7 +179,7 @@ def relative_initial_error(self) -> np.ndarray: """ Get the relative initial error. """ - return self.get_relative_error(self.initial_error) + return np.maximum(self.initial_error, 1e-3) def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> np.ndarray: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 54faf4203..942bcc3d4 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -59,12 +59,12 @@ def test_demo(self): milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) - spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.5, 2.5, 1.02]), - color=Color(0, 0, 1, 1)) - bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.2, 1.02]), + bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), color=Color(1, 1, 0, 1)) + spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.6, 2.6, 1]), + color=Color(0, 0, 1, 1)) bowl.attach(spoon) - bowl.set_position([2.5, 2.3, 1.02]) + bowl.set_position([2.5, 2.4, 1.02]) pick_pose = Pose([2.7, 2.15, 1]) From 885eb1f92d245a5436e49ad6a87de9428aa4ba29 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 14:52:07 +0200 Subject: [PATCH 072/189] [Multiverse] fixed goal validator and cache manager test issue. --- src/pycram/datastructures/world.py | 6 +++--- src/pycram/world_concepts/world_object.py | 5 ----- test/test_cache_manager.py | 10 ++++----- test/test_goal_validator.py | 26 +++++++++++------------ 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 0f6a37501..aa526839b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -161,12 +161,12 @@ class World(StateEntity, ABC): """ acceptable_position_error: float = 5e-3 # 5 cm - acceptable_orientation_error: float = 10 * np.pi / 180 # 5 degrees + acceptable_orientation_error: float = 10 * np.pi / 180 # 10 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) use_percentage_of_goal: bool = True - acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None + acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None # 50% """ - The acceptable error for the position and orientation of an object. + The acceptable error for the position and orientation of an object/link. """ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index bb313e735..9dccfdd8f 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -19,11 +19,6 @@ from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment -from ..worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ - OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, IterableErrorChecker -from ..worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, OrientationGoalValidator, \ - PositionGoalValidator, MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator, \ - MultiJointPositionGoalValidator, JointPositionGoalValidator Link = ObjectDescription.Link diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 95549d85e..b0557f971 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -2,7 +2,7 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import ObjectType -from pycram.world_concepts.world_object import Object +from pycram.object_descriptors.urdf import ObjectDescription as URDFObject import pathlib @@ -14,7 +14,7 @@ def test_generate_description_and_write_to_cache(self): path = str(file_path) + "/../resources/apartment.urdf" extension = Path(path).suffix cache_path = self.world.cache_dir + "/apartment.urdf" - apartment = Object("apartment", ObjectType.ENVIRONMENT, path) - cache_manager.generate_description_and_write_to_cache(path, apartment.name, extension, cache_path, - apartment.description) - self.assertTrue(cache_manager.is_cached(path, apartment.description)) + apartment = URDFObject(path) + cache_manager.generate_description_and_write_to_cache(path, "apartment", extension, cache_path, + apartment) + self.assertTrue(cache_manager.is_cached(path, apartment)) diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index eed9e8203..c183cd488 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -28,13 +28,13 @@ def validate_pose_goal(self, goal_validator): milk_goal_pose = Pose([1.3, 1.5, 0.9]) goal_validator.register_goal(milk_goal_pose) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0.5, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) self.milk.set_pose(milk_goal_pose) self.assertEqual(self.milk.get_pose(), milk_goal_pose) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[1], 0, places=5) @@ -50,12 +50,12 @@ def validate_position_goal(self, goal_validator): cereal_goal_position = [1.3, 1.5, 0.95] goal_validator.register_goal(cereal_goal_position) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, 0.8) self.cereal.set_position(cereal_goal_position) self.assertEqual(self.cereal.get_position_as_list(), cereal_goal_position) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 1) self.assertEqual(goal_validator.current_error, 0) def test_single_orientation_goal_generic(self): @@ -70,13 +70,13 @@ def validate_orientation_goal(self, goal_validator): cereal_goal_orientation = quaternion_from_euler(0, 0, np.pi / 2) goal_validator.register_goal(cereal_goal_orientation) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, [np.pi / 2]) self.cereal.set_orientation(cereal_goal_orientation) for v1, v2 in zip(self.cereal.get_orientation_as_list(), cereal_goal_orientation.tolist()): self.assertAlmostEqual(v1, v2, places=5) self.assertTrue(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 1) + self.assertAlmostEqual(goal_validator.actual_percentage_of_goal_achieved, 1, places=5) self.assertAlmostEqual(goal_validator.current_error.tolist()[0], 0, places=5) def test_single_revolute_joint_position_goal_generic(self): @@ -95,7 +95,7 @@ def validate_revolute_joint_position_goal(self, goal_validator, joint_type: Opti else: goal_validator.register_goal(goal_joint_position, joint_name) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: @@ -125,7 +125,7 @@ def validate_prismatic_joint_position_goal(self, goal_validator, joint_type: Opt else: goal_validator.register_goal(goal_joint_position, torso) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertEqual(goal_validator.current_error, abs(goal_joint_position)) for percent in [0.5, 1]: @@ -159,7 +159,7 @@ def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[J else: goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([0.2, abs(-np.pi / 4)]), atol=0.001)) for percent in [0.5, 1]: @@ -194,7 +194,7 @@ def validate_list_of_poses_goal(self, goal_validator): Pose(position_goal, quaternion_from_euler(*orientation_goal.tolist()))] goal_validator.register_goal(poses_goal) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue( np.allclose(goal_validator.current_error, np.array([1.0, np.pi / 2, 1.0, np.pi / 2]), atol=0.001)) @@ -232,7 +232,7 @@ def validate_list_of_positions_goal(self, goal_validator): positions_goal = [position_goal, position_goal] goal_validator.register_goal(positions_goal) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([1.0, 1.0]), atol=0.001)) for percent in [0.5, 1]: @@ -264,7 +264,7 @@ def validate_list_of_orientations_goal(self, goal_validator): quaternion_from_euler(*orientation_goal.tolist())] goal_validator.register_goal(orientations_goals) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([np.pi / 2, np.pi / 2]), atol=0.001)) for percent in [0.5, 1]: @@ -301,7 +301,7 @@ def validate_list_of_revolute_joint_positions_goal(self, goal_validator, else: goal_validator.register_goal(goal_joint_positions, joint_names) self.assertFalse(goal_validator.goal_achieved) - self.assertEqual(goal_validator.percentage_of_goal_achieved, 0) + self.assertEqual(goal_validator.actual_percentage_of_goal_achieved, 0) self.assertTrue(np.allclose(goal_validator.current_error, np.array([abs(goal_joint_position), abs(goal_joint_position)]), atol=0.001)) From e4a94e57163966b15e07855a03db4e8305c79858 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 15:16:41 +0200 Subject: [PATCH 073/189] [Multiverse] attachmed objects pose goals are registered before setting the pose of the parent. Also added a reset for the goal validator and the error checker. --- src/pycram/worlds/multiverse.py | 18 ++++-------------- .../multiverse_functions/error_checkers.py | 6 ++++++ .../multiverse_functions/goal_validator.py | 14 ++++++++++++++ test/test_cache_manager.py | 1 - 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index ed950d632..1e978e56d 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -249,7 +249,9 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): return self.pose_goal_validator.register_goal(pose, obj) - initial_attached_objects_poses = list(obj.get_poses_of_attached_objects().values()) + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) + self.multi_pose_goal_validator.register_goal(list(attachments_pose_goal.values()), + list(attachments_pose_goal.keys())) if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): @@ -258,8 +260,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): self._set_body_pose(obj.name, pose) self.pose_goal_validator.wait_until_goal_is_achieved() - if len(initial_attached_objects_poses) > 0: - self._wait_until_all_attached_objects_poses_are_set(obj, initial_attached_objects_poses) + self.multi_pose_goal_validator.wait_until_goal_is_achieved() def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: """ @@ -280,17 +281,6 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) - def _wait_until_all_attached_objects_poses_are_set(self, obj: Object, initial_poses: List[Pose]) -> None: - """ - Wait until all attached objects are set to the desired poses. - param obj: The object to which the attached objects belong. - param initial_poses: The list of initial poses of the attached objects. - """ - target_poses = obj.get_target_poses_of_attached_objects() - self.multi_pose_goal_validator.register_goal_and_wait_until_achieved(list(target_poses.values()), - list(target_poses.keys()), - initial_poses) - def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ Get the pose of a body in the simulator. diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_functions/error_checkers.py index 2b49a5787..4087befd2 100644 --- a/src/pycram/worlds/multiverse_functions/error_checkers.py +++ b/src/pycram/worlds/multiverse_functions/error_checkers.py @@ -24,6 +24,12 @@ def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterabl self.tiled_acceptable_error: Optional[np.ndarray] = None self.is_iterable = is_iterable + def reset(self) -> None: + """ + Reset the error checker. + """ + self.tiled_acceptable_error = None + @property def acceptable_error(self) -> np.ndarray: return self._acceptable_error diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index d7e43adf0..8967801b0 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -58,6 +58,8 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, :param max_wait_time: The maximum time to wait. :param time_per_read: The time to wait between each read. """ + if self.goal_value is None: + return # Skip if goal value is None start_time = time() current = self.current_value while not self.goal_achieved: @@ -70,6 +72,16 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, logging.error(msg) raise TimeoutError(msg) current = self.current_value + self.reset() + + def reset(self) -> None: + """ + Reset the goal validator. + """ + self.goal_value = None + self.initial_error = None + self.current_value_getter_input = None + self.error_checker.reset() @property def _acceptable_error(self) -> np.ndarray: @@ -106,6 +118,8 @@ def register_goal(self, goal_value: Any, :param initial_value: The initial value. :param acceptable_error: The acceptable error. """ + if goal_value is None or hasattr(goal_value, '__len__') and len(goal_value) == 0: + return # Skip if goal value is None or empty self.goal_value = goal_value self.current_value_getter_input = current_value_getter_input self.update_initial_error(goal_value, initial_value=initial_value) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index b0557f971..5e4f9876c 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,7 +1,6 @@ from pathlib import Path from bullet_world_testcase import BulletWorldTestCase -from pycram.datastructures.enums import ObjectType from pycram.object_descriptors.urdf import ObjectDescription as URDFObject import pathlib From 2dd5bdee42f5bb656d2bd48c1a6efd4ef1c1ab2b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 18:18:00 +0200 Subject: [PATCH 074/189] [Multiverse] made the validation into decorators. --- src/pycram/datastructures/world.py | 56 +++++++++---- src/pycram/world_concepts/world_object.py | 25 +++--- src/pycram/worlds/bullet_world.py | 4 +- src/pycram/worlds/multiverse.py | 52 +++--------- .../multiverse_functions/goal_validator.py | 82 +++++++++++++++++-- test/test_multiverse.py | 5 +- 6 files changed, 140 insertions(+), 84 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index aa526839b..6eabee2ee 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -11,8 +11,7 @@ import rospy from geometry_msgs.msg import Point from tf.transformations import euler_from_quaternion -from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING -from typing_extensions import Union +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union from ..cache_manager import CacheManager from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, @@ -27,9 +26,9 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event -from ..worlds.multiverse_functions.goal_validator import GoalValidator, MultiPoseGoalValidator, \ +from ..worlds.multiverse_functions.goal_validator import MultiPoseGoalValidator, \ MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ - OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator + OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -169,6 +168,11 @@ class World(StateEntity, ABC): The acceptable error for the position and orientation of an object/link. """ + raise_goal_validator_error: Optional[bool] = False + """ + Whether to raise an error if the goals are not achieved. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): """ Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -185,6 +189,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.let_pycram_handle_spawning = True self.let_pycram_handle_world_sync = True self.update_poses_from_sim_on_get = True + GoalValidator.raise_error = World.raise_goal_validator_error if World.current_world is None: World.current_world = self @@ -261,10 +266,17 @@ def _init_goal_validators(self): # Goal validators for the joints of an object self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) + self.get_joint_position, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), - acceptable_percentage_of_goal_achieved= self.acceptable_percentage_of_goal) + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + + def check_object_exists(self, obj: Object): + """ + Checks if the object exists in the simulator and issues a warning if it does not. + :param obj: The object to check. + """ + return obj not in self.objects @abstractmethod def _init_world(self, mode: WorldMode): @@ -643,7 +655,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal, validate=False) + self.robot.set_joint_positions(goal) def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ @@ -754,12 +766,11 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: pass @abstractmethod - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. - :param validate: If the joint position goals should be validated. + :return: True if the set was successful, False otherwise. """ pass @@ -771,13 +782,14 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: pass @abstractmethod - def reset_object_base_pose(self, obj: Object, pose: Pose): + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. :param obj: The object. :param pose: The new pose as a Pose object. + :return: True if the reset was successful, False otherwise. """ pass @@ -1235,7 +1247,8 @@ def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: """ Creates a box visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the box visual shape to be created, uses the BoxVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1244,7 +1257,8 @@ def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: """ Creates a cylinder visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the cylinder visual shape to be created, uses the CylinderVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the cylinder visual shape to be created, uses the + CylinderVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1253,7 +1267,8 @@ def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: """ Creates a sphere visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the sphere visual shape to be created, uses the SphereVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1262,7 +1277,8 @@ def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: """ Creates a capsule visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the capsule visual shape to be created, uses the CapsuleVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the capsule visual shape to be created, uses the + CapsuleVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1271,7 +1287,8 @@ def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: """ Creates a plane visual shape in the physics simulator and returns the unique id of the created shape. - :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape dataclass defined in world_dataclasses. + :param shape_data: The parameters that define the plane visual shape to be created, uses the PlaneVisualShape + dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ raise NotImplementedError @@ -1294,10 +1311,13 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :param text: The text to be added. :param position: The position of the text in the world. - :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a text orientation (quaternion), the orientation will be fixed in world space or local space (when parent is specified). + :param orientation: By default, debug text will always face the camera, automatically rotation. By specifying a + text orientation (quaternion), the orientation will be fixed in world space or local space + (when parent is specified). :param size: The size of the text. :param color: The color of the text. - :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the world until it is removed manually. + :param life_time: The lifetime in seconds of the text to remain in the world, if 0 the text will remain in the + world until it is removed manually. :param parent_object_id: The id of the object to which the text should be attached. :param parent_link_id: The id of the link to which the text should be attached. :return: The id of the added text. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9dccfdd8f..d2016db43 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -61,8 +61,10 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if pose is None: pose = Pose() if name in [obj.name for obj in self.world.objects]: - rospy.logerr(f"An object with the name {name} already exists in the world.") - raise ValueError(f"An object with the name {name} already exists in the world.") + msg = f"An object with the name {name} already exists in the world,"\ + f" is_prospection_world: {self.world.is_prospection_world}" + rospy.logerr(msg) + raise ValueError(msg) self.name: str = name self.obj_type: ObjectType = obj_type @@ -203,7 +205,7 @@ def _add_virtual_move_base_joints(self): child_link = self.root_link_name axes = virtual_joints.get_axes() for joint_name, joint_type in virtual_joints.get_types().items(): - self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name]) + self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name], is_virtual=True) def _init_joint_name_and_id_map(self) -> None: """ @@ -590,8 +592,8 @@ def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Op self._set_attached_objects_poses() def reset_base_pose(self, pose: Pose): - self.world.reset_object_base_pose(self, pose) - self.update_pose() + if self.world.reset_object_base_pose(self, pose): + self.update_pose() def update_pose(self): """ @@ -908,21 +910,20 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float], validate: Optional[bool] = True) -> None: + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_positions: A dictionary with the joint names as keys and the target positions as values. - :param validate: If True the joint position goals will be validated. """ joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} - self.world.set_multiple_joint_positions(joint_positions, validate) - self.update_pose() - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() + if self.world.set_multiple_joint_positions(joint_positions): + self.update_pose() + self._update_all_links_poses() + self.update_link_transforms() + self._set_attached_objects_poses() def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index e809aff2a..d9554daf0 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -182,10 +182,10 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) + return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 1e978e56d..d5a8f6d95 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,10 +3,12 @@ from time import sleep, time import numpy as np +import rospy from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -198,19 +200,12 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> None: {self.get_joint_position_name(joint): [joint_position]}) self.joint_position_goal_validator.wait_until_goal_is_achieved() - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float], - validate: Optional[bool] = True) -> None: + @validate_multiple_joint_positions + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} for joint, position in joint_positions.items()} - - if validate: - self.multi_joint_position_goal_validator.register_goal(list(joint_positions.values()), - [joint.type for joint in joint_positions.keys()], - list(joint_positions.keys())) self.writer.send_multiple_body_data_to_server(data) - - if validate: - self.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + return True def get_joint_position(self, joint: Joint) -> Optional[float]: data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) @@ -218,22 +213,19 @@ def get_joint_position(self, joint: Joint) -> Optional[float]: return data[0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: - self.check_object_exists_and_issue_warning_if_not(joints[0].object) joint_names = [joint.name for joint in joints] data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] for joint in joints}) if data is not None: return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} - def get_link_pose(self, link: Link) -> Pose: - self.check_object_exists_and_issue_warning_if_not(link.object) + def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: return self._get_multiple_body_poses([link.name for link in links]) def get_object_pose(self, obj: Object) -> Pose: - self.check_object_exists_and_issue_warning_if_not(obj) if obj.obj_type == ObjectType.ENVIRONMENT: return Pose() return self._get_body_pose(obj.name) @@ -241,17 +233,10 @@ def get_object_pose(self, obj: Object) -> Pose: def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: return self._get_multiple_body_poses([obj.name for obj in objects]) - def reset_object_base_pose(self, obj: Object, pose: Pose): - - self.check_object_exists_and_issue_warning_if_not(obj) - + @validate_object_pose + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: if obj.has_type_environment(): - return - - self.pose_goal_validator.register_goal(pose, obj) - attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) - self.multi_pose_goal_validator.register_goal(list(attachments_pose_goal.values()), - list(attachments_pose_goal.keys())) + return False if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): @@ -259,8 +244,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose): else: self._set_body_pose(obj.name, pose) - self.pose_goal_validator.wait_until_goal_is_achieved() - self.multi_pose_goal_validator.wait_until_goal_is_achieved() + return True def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: """ @@ -339,9 +323,6 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError - self.check_object_exists_and_issue_warning_if_not(constraint.parent_link.object) - self.check_object_exists_and_issue_warning_if_not(constraint.child_link.object) - if not self.let_pycram_move_attached_objects: self.api_requester.attach(constraint) @@ -368,7 +349,6 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - self.check_object_exists_and_issue_warning_if_not(obj) multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -411,8 +391,6 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L return contact_force_array.flatten().tolist()[2] def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: - self.check_object_exists_and_issue_warning_if_not(obj1) - self.check_object_exists_and_issue_warning_if_not(obj2) obj1_contact_points = self.get_object_contact_points(obj1) return obj1_contact_points.get_points_of_object(obj2) @@ -450,26 +428,21 @@ def restore_physics_simulator_state(self, state_id: int) -> None: raise NotImplementedError def set_link_color(self, link: Link, rgba_color: Color): - self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("set_link_color is not implemented in Multiverse") def get_link_color(self, link: Link) -> Color: - self.check_object_exists_and_issue_warning_if_not(link.object) logging.warning("get_link_color is not implemented in Multiverse") return Color() def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - self.check_object_exists_and_issue_warning_if_not(obj) logging.warning("get_colors_of_object_links is not implemented in Multiverse") return {} def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - self.check_object_exists_and_issue_warning_if_not(obj) logging.error("get_object_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - self.check_object_exists_and_issue_warning_if_not(link.object) logging.error("get_link_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError @@ -479,11 +452,6 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") - def check_object_exists_and_issue_warning_if_not(self, obj: Object): - if obj not in self.objects: - msg = f"Object {obj} does not exist in the simulator" - logging.warning(msg) - def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index 8967801b0..e2c269f12 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -1,13 +1,22 @@ -import logging from time import sleep, time import numpy as np -from typing_extensions import Any, Callable, Optional, Union, Iterable +import rospy +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING from pycram.datastructures.enums import JointType from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker +if TYPE_CHECKING: + from pycram.datastructures.world import World + from pycram.world_concepts.world_object import Object + from pycram.datastructures.pose import Pose + from pycram.description import ObjectDescription + + Joint = ObjectDescription.Joint + Link = ObjectDescription.Link + OptionalArgCallable = Union[Callable[[], Any], Callable[[Any], Any]] @@ -16,6 +25,11 @@ class GoalValidator: A class to validate the goal by tracking the goal achievement progress. """ + raise_error: Optional[bool] = False + """ + Whether to raise an error if the goal is not achieved. + """ + def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalArgCallable, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ @@ -37,7 +51,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, current_value_getter_input: Optional[Any] = None, initial_value: Optional[Any] = None, acceptable_error: Optional[Union[float, Iterable[float]]] = None, - max_wait_time: Optional[float] = 2, + max_wait_time: Optional[float] = 1, time_per_read: Optional[float] = 0.01) -> None: """ Register the goal value and wait until the target is reached. @@ -51,7 +65,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) self.wait_until_goal_is_achieved(max_wait_time, time_per_read) - def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 1, time_per_read: Optional[float] = 0.01) -> None: """ Wait until the target is reached. @@ -69,8 +83,12 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, f" goal {self.goal_value} within {max_wait_time}" \ f" seconds, the current value is {current}, error is {self.current_error}, percentage" \ f" of goal achieved is {self.percentage_of_goal_achieved}" - logging.error(msg) - raise TimeoutError(msg) + if self.raise_error: + rospy.logerr(msg) + raise TimeoutError(msg) + else: + rospy.logwarn(msg) + break current = self.current_value self.reset() @@ -250,7 +268,7 @@ class MultiPoseGoalValidator(PoseGoalValidator): """ def __init__(self, current_poses_getter: OptionalArgCallable = None, - acceptable_error: Union[float, Iterable[float]] = (1e-2, 5*np.pi / 180), + acceptable_error: Union[float, Iterable[float]] = (1e-2, 5 * np.pi / 180), acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-pose goal validator. @@ -406,3 +424,53 @@ def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_type] super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) + + +def validate_object_pose(pose_setter_func): + """ + A decorator to validate the object pose. + :param pose_setter_func: The function to set the pose of the object. + """ + + def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) + if len(attachments_pose_goal) == 0: + world.pose_goal_validator.register_goal(pose, obj) + else: + world.multi_pose_goal_validator.register_goal([pose, *list(attachments_pose_goal.values())], + [obj, *list(attachments_pose_goal.keys())]) + if not pose_setter_func(world, obj, pose): + world.pose_goal_validator.reset() + world.multi_pose_goal_validator.reset() + return False + + world.pose_goal_validator.wait_until_goal_is_achieved() + world.multi_pose_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_multiple_joint_positions(position_setter_func): + """ + A decorator to validate the joint positions, this function does not validate the virtual joints, + as in multiverse the virtual joints take command velocities and not positions, so after their goals + are set, they are zeroed thus can't be validated. (They are actually validated by the robot pose in case + of virtual move base joints) + :param position_setter_func: The function to set the joint positions. + """ + + def wrapper(world: 'World', joint_positions: Dict['Joint', float]): + joint_positions_to_validate = {joint: position for joint, position in joint_positions.items() + if not joint.is_virtual} + joint_types = [joint.type for joint in joint_positions_to_validate.keys()] + world.multi_joint_position_goal_validator.register_goal(list(joint_positions_to_validate.values()), joint_types, + list(joint_positions_to_validate.keys())) + if not position_setter_func(world, joint_positions): + world.multi_joint_position_goal_validator.reset() + return False + + world.multi_joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 942bcc3d4..4edd5e431 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -200,11 +200,10 @@ def test_attach_with_robot(self): robot = self.spawn_robot() ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector - robot.attach(milk, ee_link.name, coincide_the_objects=True) + robot.attach(milk, ee_link.name, coincide_the_objects=False) self.assertTrue(robot in milk.attachments) milk_initial_pose = milk.root_link.get_pose_wrt_link(ee_link) - robot_position = robot.get_joint_position("arm_right_2_joint") - robot_position += 1.57 + robot_position = 1.57 robot.set_joint_position("arm_right_2_joint", robot_position) milk_pose = milk.root_link.get_pose_wrt_link(ee_link) self.assert_poses_are_equal(milk_initial_pose, milk_pose) From c1eda3b600ab90b6abc6481b50d74beef4400a5e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 24 Jul 2024 18:26:38 +0200 Subject: [PATCH 075/189] [Multiverse] better angle calculation for mobile robot pose goal. --- src/pycram/datastructures/world.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 6eabee2ee..c023ae8f3 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -26,6 +26,8 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event +from ..worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + calculate_quaternion_difference from ..worlds.multiverse_functions.goal_validator import MultiPoseGoalValidator, \ MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator @@ -698,9 +700,8 @@ def get_z_angle_diff(current_quaternion: List[float], target_quaternion: List[fl param target_quaternion: The target quaternion. return: The difference between the z angles of the two quaternions in euler angles. """ - current_angle = euler_from_quaternion(current_quaternion)[2] - target_angle = euler_from_quaternion(target_quaternion)[2] - return target_angle - current_angle + quat_diff = calculate_quaternion_difference(current_quaternion, target_quaternion) + return euler_from_quaternion(quat_diff)[2] @abstractmethod def perform_collision_detection(self) -> None: From dd712e0367c8c375613292ce9b46fd2272712f1a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 11:31:56 +0200 Subject: [PATCH 076/189] [Multiverse] execute set_robot_position multiple times in testing. --- test/test_multiverse.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 030208b1f..c904a976a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -156,12 +156,14 @@ def test_respawn_robot(self): # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): - self.spawn_robot() - new_position = [-3, -3, 0.001] - # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) - self.multiverse.robot.set_position(new_position) - robot_position = self.multiverse.robot.get_position_as_list() - self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + for i in range(10): + self.spawn_robot() + new_position = [-3, -3, 0.001] + # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) + self.multiverse.robot.set_position(new_position) + robot_position = self.multiverse.robot.get_position_as_list() + self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + self.tearDown() def test_attach_object(self): milk = self.spawn_milk([1, 0, 0.1]) From 09fca2cc0aba2c69ae2e7f25d1dcd60124064773 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 12:37:47 +0200 Subject: [PATCH 077/189] [Multiverse] corrected mobile robot orientation target, tests are working. --- src/pycram/datastructures/world.py | 20 ++++++------ src/pycram/world_concepts/world_object.py | 2 +- .../multiverse_communication/clients.py | 18 ++++------- .../multiverse_functions/goal_validator.py | 19 +++++++++-- test/test_multiverse.py | 32 ++++++++++++++----- 5 files changed, 58 insertions(+), 33 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index c023ae8f3..b4d0834c5 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -268,9 +268,13 @@ def _init_goal_validators(self): # Goal validators for the joints of an object self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + self.get_joint_position, acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), + acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) def check_object_exists(self, obj: Object): @@ -666,12 +670,12 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: return: The goal for the move base joints. """ position_diff = self.get_position_diff(self.robot.get_position_as_list(), pose.position_as_list())[:2] - angle_diff = self.get_z_angle_diff(self.robot.get_orientation_as_list(), pose.orientation_as_list()) + target_angle = self.get_z_angle(pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() return {move_base_joints.translation_x: position_diff[0], move_base_joints.translation_y: position_diff[1], - move_base_joints.angular_z: angle_diff} + move_base_joints.angular_z: target_angle} @staticmethod def get_move_base_joints() -> VirtualMoveBaseJoints: @@ -693,15 +697,13 @@ def get_position_diff(current_position: List[float], target_position: List[float return [target_position[i] - current_position[i] for i in range(3)] @staticmethod - def get_z_angle_diff(current_quaternion: List[float], target_quaternion: List[float]) -> float: + def get_z_angle(target_quaternion: List[float]) -> float: """ - Get the difference between the z angles of two quaternions. - param current_quaternion: The current quaternion. + Get the z angle from a quaternion by converting it to euler angles. param target_quaternion: The target quaternion. - return: The difference between the z angles of the two quaternions in euler angles. + return: The z angle. """ - quat_diff = calculate_quaternion_difference(current_quaternion, target_quaternion) - return euler_from_quaternion(quat_diff)[2] + return euler_from_quaternion(target_quaternion)[2] @abstractmethod def perform_collision_detection(self) -> None: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d2016db43..44e7b0f02 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -904,7 +904,7 @@ def reset_all_joints_positions(self) -> None: """ Sets the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = [joint.name for joint in self.joints.values() if not joint.is_virtual] + joint_names = [joint.name for joint in self.joints.values()] # if not joint.is_virtual] if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 718fd3ad3..470250e3f 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -262,15 +262,6 @@ def join(self): class MultiverseWriter(MultiverseClient): - time_for_sim_update: Optional[float] = 0.0 # 0.02 - """ - Wait time for the sent data to be applied in the simulator. - """ - time_for_setting_body_data: Optional[float] = 0.0 # 0.01 - """ - Wait time for setting body data. - """ - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ @@ -365,7 +356,6 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_setting_body_data * self.simulation_wait_time_factor) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -397,7 +387,6 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() - sleep(self.time_for_sim_update * self.simulation_wait_time_factor) return self.response_meta_data @@ -411,6 +400,7 @@ class MultiverseAPI(MultiverseClient): """ The wait time for the API request in seconds. """ + APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH] def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0): @@ -426,12 +416,14 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation + self.wait: Optional[bool] = False # Whether to wait after sending the API request. def attach(self, constraint: Constraint) -> None: """ Request to attach the child link to the parent link. param constraint: The constraint. """ + self.wait = True parent_link_name, child_link_name = self.get_constraint_link_names(constraint) attachment_pose = self._get_attachment_pose_as_string(constraint) self._attach(child_link_name, parent_link_name, attachment_pose) @@ -630,7 +622,9 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() - sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) + if self.wait: + sleep(self.API_REQUEST_WAIT_TIME * self.simulation_wait_time_factor) + self.wait = False return responses def _get_all_apis_responses(self) -> Dict[API, List[str]]: diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index e2c269f12..b59e70168 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -368,6 +368,8 @@ class JointPositionGoalValidator(GoalValidator): def __init__(self, current_position_getter: OptionalArgCallable = None, acceptable_error: Optional[float] = None, + acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, + acceptable_position_error: Optional[Iterable[float]] = 1e-3, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, is_iterable: Optional[bool] = False): """ @@ -375,11 +377,15 @@ def __init__(self, current_position_getter: OptionalArgCallable = None, :param current_position_getter: The current position getter function which takes an optional input and returns the current position. :param acceptable_error: The acceptable error. + :param acceptable_orientation_error: The acceptable orientation error. + :param acceptable_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. :param is_iterable: Whether it is a sequence of joint positions. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_orientation_error + self.acceptable_position_error = acceptable_position_error def register_goal(self, goal_value: Any, joint_type: JointType, current_value_getter_input: Optional[Any] = None, @@ -394,7 +400,8 @@ def register_goal(self, goal_value: Any, joint_type: JointType, :param acceptable_error: The acceptable error. """ if acceptable_error is None: - self.error_checker.acceptable_error = np.pi / 180 if joint_type == JointType.REVOLUTE else 1e-3 + self.error_checker.acceptable_error = self.acceptable_orientation_error if joint_type == JointType.REVOLUTE\ + else self.acceptable_position_error super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) @@ -405,24 +412,30 @@ class MultiJointPositionGoalValidator(GoalValidator): def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_error: Optional[Iterable[float]] = None, + acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, + acceptable_position_error: Optional[Iterable[float]] = 1e-3, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-joint position goal validator. :param current_positions_getter: The current positions getter function which takes an optional input and returns the current positions. :param acceptable_error: The acceptable error. + :param acceptable_orientation_error: The acceptable orientation error. + :param acceptable_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, acceptable_percentage_of_goal_achieved) + self.acceptable_orientation_error = acceptable_orientation_error + self.acceptable_position_error = acceptable_position_error def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], current_value_getter_input: Optional[Any] = None, initial_value: Optional[Any] = None, acceptable_error: Optional[Iterable[float]] = None): if acceptable_error is None: - self.error_checker.acceptable_error = [np.pi / 180 if jt == JointType.REVOLUTE else 1e-3 for jt in - joint_type] + self.error_checker.acceptable_error = [self.acceptable_orientation_error if jt == JointType.REVOLUTE + else self.acceptable_position_error for jt in joint_type] super().register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4edd5e431..44c6c7084 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -2,7 +2,10 @@ import os import unittest +import numpy as np import psutil +from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply, quaternion_inverse, \ + quaternion_conjugate from typing_extensions import Optional, List from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint @@ -11,6 +14,8 @@ from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ + calculate_quaternion_difference multiverse_installed = True try: @@ -136,7 +141,6 @@ def test_spawn_robot(self): self.assertTrue(robot in self.multiverse.objects) self.assertTrue(self.multiverse.robot.name == robot.name) - # @unittest.skip("Not implemented feature yet.") def test_destroy_robot(self): if self.multiverse.robot is None: self.spawn_robot() @@ -152,14 +156,27 @@ def test_respawn_robot(self): self.spawn_robot() self.assertTrue(self.multiverse.robot in self.multiverse.objects) - # @unittest.skip("This will cause respawning of the robot.") def test_set_robot_position(self): + for i in range(3): + self.spawn_robot() + new_position = [-3, -3, 0.001] + self.multiverse.robot.set_position(new_position) + robot_position = self.multiverse.robot.get_position_as_list() + self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + self.tearDown() + + def test_set_robot_orientation(self): self.spawn_robot() - new_position = [-3, -3, 0.001] - # self.multiverse.writer.send_multiple_body_data_to_server({"odom_vel_lin_x_joint": {"joint_tvalue": [-4]}}) - self.multiverse.robot.set_position(new_position) - robot_position = self.multiverse.robot.get_position_as_list() - self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + for i in range(3): + current_quaternion = self.multiverse.robot.get_orientation_as_list() + # rotate by 45 degrees without using euler angles + rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) + new_quaternion = quaternion_multiply(current_quaternion, rotation_quaternion) + self.multiverse.robot.set_orientation(new_quaternion) + robot_orientation = self.multiverse.robot.get_orientation_as_list() + quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) + self.assertAlmostEqual(quaternion_difference, 0, delta=0.01) + # self.tearDown() def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) @@ -175,7 +192,6 @@ def test_attach_object(self): new_cup_position = cup.get_position_as_list() self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2]) - # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): for i in range(2): milk = self.spawn_milk([1, 0, 0.1]) From 784c33ca64602c28ec5b3fa40f52ff0ecd55f09a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 17:23:58 +0200 Subject: [PATCH 078/189] [Multiverse] defined enums for multiverse properties, and did some cleaning. set single joint position test is not passing because joints currently does not retain their positions, will be fixed soon. --- src/pycram/datastructures/world.py | 60 ++++----------- .../location/giskard_location.py | 2 +- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/pose_generator_and_validator.py | 6 +- .../process_modules/boxy_process_modules.py | 14 ++-- .../process_modules/donbot_process_modules.py | 12 +-- .../process_modules/hsrb_process_modules.py | 6 +- .../process_modules/pr2_process_modules.py | 6 +- .../stretch_process_modules.py | 2 +- src/pycram/utils.py | 2 +- src/pycram/world_concepts/world_object.py | 25 +++---- src/pycram/worlds/bullet_world.py | 12 +-- src/pycram/worlds/multiverse.py | 68 +++++++++-------- .../multiverse_communication/clients.py | 75 ++++++++++++------- .../worlds/multiverse_datastructures/enums.py | 25 +++++++ test/test_goal_validator.py | 4 +- 16 files changed, 164 insertions(+), 157 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b4d0834c5..e1ca2f471 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -228,49 +228,17 @@ def _init_goal_validators(self): """ Initializes the goal validators for the World objects' poses, positions, and orientations. """ + + # Goal validators for an object + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, + self.acceptable_percentage_of_goal) + # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error, self.acceptable_percentage_of_goal) - self.multi_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_object_positions(x).values()), - self.acceptable_position_error, self.acceptable_percentage_of_goal) - self.multi_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_object_orientations(x).values()), - self.acceptable_orientation_error, self.acceptable_percentage_of_goal) - # Goal validators for an object - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, - self.acceptable_percentage_of_goal) - self.position_goal_validator = PositionGoalValidator(self.get_object_position, self.acceptable_position_error, - self.acceptable_percentage_of_goal) - self.orientation_goal_validator = OrientationGoalValidator(self.get_object_orientation, - self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - # Goal validators for the links of an object - self.link_pose_goal_validator = PoseGoalValidator(self.get_link_pose, self.acceptable_pose_error) - self.link_position_goal_validator = PositionGoalValidator(self.get_link_position, - self.acceptable_position_error) - self.link_orientation_goal_validator = OrientationGoalValidator(self.get_link_orientation, - self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - self.multi_link_pose_goal_validator = MultiPoseGoalValidator( - lambda x: list(self.get_multiple_link_poses(x).values()), self.acceptable_pose_error, - self.acceptable_percentage_of_goal) - self.multi_link_position_goal_validator = MultiPositionGoalValidator( - lambda x: list(self.get_multiple_link_positions(x).values()), self.acceptable_position_error, - self.acceptable_percentage_of_goal) - self.multi_link_orientation_goal_validator = MultiOrientationGoalValidator( - lambda x: list(self.get_multiple_link_orientations(x).values()), self.acceptable_orientation_error, - self.acceptable_percentage_of_goal) - - # Goal validators for the joints of an object - self.joint_position_goal_validator = JointPositionGoalValidator( - self.get_joint_position, acceptable_orientation_error=self.acceptable_orientation_error, - acceptable_position_error=self.acceptable_position_error, - acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + # Goal validator for the joints of an object self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), acceptable_orientation_error=self.acceptable_orientation_error, @@ -661,7 +629,7 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_joint_positions(goal) + self.robot.set_multiple_joint_positions(goal) def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ @@ -758,15 +726,15 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError - @abstractmethod - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation :param joint: The joint to reset the position for. :param joint_position: The new joint pose. + :return: True if the reset was successful, False otherwise """ - pass + return self.set_multiple_joint_positions({joint: joint_position}) @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: @@ -784,7 +752,6 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass - @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, @@ -794,15 +761,16 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. """ - pass + return self.reset_multiple_objects_base_poses({obj: pose}) @abstractmethod - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: """ Reset the world position and orientation of the base of multiple objects instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. :param objects: A dictionary with objects as keys and poses as values. + :return: True if the reset was successful, False otherwise. """ pass @@ -951,7 +919,7 @@ def reset_current_world(self) -> None: """ for obj in self.objects: obj.set_pose(obj.original_pose) - obj.set_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) + obj.set_multiple_joint_positions(dict(zip(list(obj.joint_names), [0] * len(obj.joint_names)))) def reset_robot(self) -> None: """ diff --git a/src/pycram/designators/specialized_designators/location/giskard_location.py b/src/pycram/designators/specialized_designators/location/giskard_location.py index 1400a8e63..de0d0a6e8 100644 --- a/src/pycram/designators/specialized_designators/location/giskard_location.py +++ b/src/pycram/designators/specialized_designators/location/giskard_location.py @@ -53,7 +53,7 @@ def __iter__(self) -> CostmapLocation.Location: prospection_robot = World.current_world.get_prospection_object_for_object(World.robot) with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) gripper_pose = prospection_robot.get_link_pose(chain.get_tool_frame()) diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 17ceca769..8f59db07a 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -234,7 +234,7 @@ def request_giskard_ik(target_pose: Pose, robot: Object, gripper: str) -> Tuple[ robot_joint_states[joint_name] = state with UseProspectionWorld(): - prospection_robot.set_joint_positions(robot_joint_states) + prospection_robot.set_multiple_joint_positions(robot_joint_states) prospection_robot.set_pose(pose) tip_pose = prospection_robot.get_link_pose(gripper) diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 170362783..bb19fa0ff 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -203,14 +203,14 @@ def reachability_validator(pose: Pose, # test the possible solution and apply it to the robot pose, joint_states = request_ik(target, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: # only check for retract pose if pose worked pose, joint_states = request_ik(retract_target_pose, robot, joints, tool_frame) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) # _apply_ik(robot, resp, joints) in_contact = collision_check(robot, allowed_collision) if not in_contact: @@ -218,7 +218,7 @@ def reachability_validator(pose: Pose, except IKError: pass finally: - robot.set_joint_positions(joint_state_before_ik) + robot.set_multiple_joint_positions(joint_state_before_ik) if arms: res = True return res, arms diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 5dc25d8f9..d2fcccf8d 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -89,13 +89,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("neck", "neck_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("neck_shoulder_link")) @@ -115,7 +115,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.kinematic_chains[gripper].get_static_gripper_state(motion)) class BoxyDetecting(ProcessModule): @@ -160,9 +160,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class BoxyWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index e9ff45edc..ae9fb70b9 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -69,13 +69,13 @@ def _execute(self, desig): pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) if pose_in_shoulder.position.x >= 0 and pose_in_shoulder.position.x >= abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "front")) if pose_in_shoulder.position.y >= 0 and pose_in_shoulder.position.y >= abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_right")) if pose_in_shoulder.position.x <= 0 and abs(pose_in_shoulder.position.x) > abs(pose_in_shoulder.position.y): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "back")) if pose_in_shoulder.position.y <= 0 and abs(pose_in_shoulder.position.y) > abs(pose_in_shoulder.position.x): - robot.set_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_static_joint_chain("left", "arm_left")) pose_in_shoulder = local_transformer.transform_pose(target, robot.get_link_tf_frame("ur5_shoulder_link")) @@ -94,7 +94,7 @@ def _execute(self, desig): robot = World.robot gripper = desig.gripper motion = desig.motion - robot.set_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) + robot.set_multiple_joint_positions(RobotDescription.current_robot_description.get_arm_chain(gripper).get_static_gripper_state(motion)) class DonbotMoveTCP(ProcessModule): @@ -118,7 +118,7 @@ class DonbotMoveJoints(ProcessModule): def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class DonbotWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/hsrb_process_modules.py b/src/pycram/process_modules/hsrb_process_modules.py index 5b66d6ec2..cdab22128 100644 --- a/src/pycram/process_modules/hsrb_process_modules.py +++ b/src/pycram/process_modules/hsrb_process_modules.py @@ -152,9 +152,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class HSRBMoveJoints(ProcessModule): @@ -164,7 +164,7 @@ class HSRBMoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion): robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) + robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions))) class HSRBWorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 0ac65ba8d..62112ec5e 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -121,9 +121,9 @@ def _execute(self, desig: MoveArmJointsMotion): robot = World.robot if desig.right_arm_poses: - robot.set_joint_positions(desig.right_arm_poses) + robot.set_multiple_joint_positions(desig.right_arm_poses) if desig.left_arm_poses: - robot.set_joint_positions(desig.left_arm_poses) + robot.set_multiple_joint_positions(desig.left_arm_poses) class PR2MoveJoints(ProcessModule): @@ -133,7 +133,7 @@ class PR2MoveJoints(ProcessModule): def _execute(self, desig: MoveJointsMotion): robot = World.robot - robot.set_joint_positions(dict(zip(desig.names, desig.positions))) + robot.set_multiple_joint_positions(dict(zip(desig.names, desig.positions))) class Pr2WorldStateDetecting(ProcessModule): diff --git a/src/pycram/process_modules/stretch_process_modules.py b/src/pycram/process_modules/stretch_process_modules.py index 77b33596a..6ac3d45df 100644 --- a/src/pycram/process_modules/stretch_process_modules.py +++ b/src/pycram/process_modules/stretch_process_modules.py @@ -132,7 +132,7 @@ def _move_arm_tcp(target: Pose, robot: Object, arm: Arms) -> None: # inv = request_ik(target, robot, joints, gripper) pose, joint_states = request_giskard_ik(target, robot, gripper) robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) ########################################################### diff --git a/src/pycram/utils.py b/src/pycram/utils.py index 28b5109ac..c4cb7710e 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -46,7 +46,7 @@ def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses: """ pose, joint_states = pose_and_joint_poses robot.set_pose(pose) - robot.set_joint_positions(joint_states) + robot.set_multiple_joint_positions(joint_states) class GeneratorList: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 44e7b0f02..ece5eda96 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -908,9 +908,18 @@ def reset_all_joints_positions(self) -> None: if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) - self.set_joint_positions(dict(zip(joint_names, joint_positions))) + self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + def set_joint_position(self, joint_name: str, joint_position: float) -> None: + """ + Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. + + :param joint_name: The name of the joint + :param joint_position: The target pose for this joint + """ + self.world.reset_joint_position(self.joints[joint_name], joint_position) + + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. @@ -925,18 +934,6 @@ def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: self.update_link_transforms() self._set_attached_objects_poses() - def set_joint_position(self, joint_name: str, joint_position: float) -> None: - """ - Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. - - :param joint_name: The name of the joint - :param joint_position: The target pose for this joint - """ - self.joints[joint_name].position = joint_position - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() - def get_joint_position(self, joint_name: str) -> float: """ :param joint_name: The name of the joint diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index d9554daf0..253776796 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -184,22 +184,16 @@ def parse_points_list_to_args(self, point: List) -> Dict: def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): - self.reset_joint_position(joint, joint_position) + p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} - def reset_joint_position(self, joint: ObjectDescription.Joint, joint_position: float) -> None: - p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) - - def reset_object_base_pose(self, obj: Object, pose: Pose) -> None: - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), - physicsClientId=self.id) - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: for obj, pose in objects.items(): - self.reset_object_base_pose(obj, pose) + p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + physicsClientId=self.id) def step(self): p.stepSimulation(physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d5a8f6d95..854a080fe 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,15 +1,13 @@ import logging import os -from time import sleep, time import numpy as np -import rospy from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions - from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -25,9 +23,9 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ - _joint_type_to_position_name: Dict[JointType, str] = { - JointType.REVOLUTE: "joint_rvalue", - JointType.PRISMATIC: "joint_tvalue", + _joint_type_to_position_name: Dict[JointType, MultiverseJointProperty] = { + JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_POSITION, + JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION, } """ A dictionary to map JointType to the corresponding multiverse attribute name. @@ -140,10 +138,9 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def get_joint_position_name(self, joint: Joint) -> str: + def get_joint_position_name(self, joint: Joint) -> MultiverseJointProperty: if joint.type not in self._joint_type_to_position_name: - logging.warning(f"Invalid joint type: {joint.type}") - return "joint_rvalue" + raise ValueError(f"Joint type {joint.type} is not supported in Multiverse") return self._joint_type_to_position_name[joint.type] def load_object_and_get_id(self, name: Optional[str] = None, @@ -194,12 +191,6 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) - def reset_joint_position(self, joint: Joint, joint_position: float) -> None: - self.joint_position_goal_validator.register_goal(joint_position, joint.type, joint) - self.writer.send_body_data_to_server(joint.name, - {self.get_joint_position_name(joint): [joint_position]}) - self.joint_position_goal_validator.wait_until_goal_is_achieved() - @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} @@ -208,16 +199,16 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b return True def get_joint_position(self, joint: Joint) -> Optional[float]: - data = self.reader.get_body_property(joint.name, self.get_joint_position_name(joint)) - if data is not None and len(data) > 0: - return data[0] + data = self.get_multiple_joint_positions([joint]) + if data is not None: + return data[joint.name] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] for joint in joints}) if data is not None: - return {joint_name: list(data[joint_name].values())[0][0] for joint_name in joint_names} + return {name: list(value.values())[0][0] for name, value in data.items()} def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) @@ -260,10 +251,29 @@ def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: return False def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: - # TODO: Implement a more efficient way to reset multiple objects' poses by sending all the poses at once, - # instead of sending them one by one, this can be done constructing the metadata and data dictionaries. - for obj, pose in objects.items(): - self.reset_object_base_pose(obj, pose) + """ + Reset the poses of multiple objects in the simulator. + param objects: The dictionary of objects and poses. + """ + self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) + + def _set_body_pose(self, body_name: str, pose: Pose) -> None: + """ + Reset the pose of a body (object, link, or joint) in the simulator. + param body_name: The name of the body. + param pose: The pose of the body. + """ + self._set_multiple_body_poses({body_name: pose}) + + def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: + """ + Reset the poses of multiple bodies in the simulator. + param body_poses: The dictionary of body names and poses. + """ + self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), + MultiverseBodyProperty.ORIENTATION: + self.xyzw_to_wxyz(pose.orientation_as_list())} + for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ @@ -292,15 +302,9 @@ def get_object_orientation(self, obj: Object) -> List[float]: def multiverse_reset_world(self): self.writer.reset_world() - def _set_body_pose(self, body_name: str, pose: Pose) -> None: - """ - Reset the pose of a body (object, link, or joint) in the simulator. - param body_name: The name of the body. - param pose: The pose of the body. - """ - xyzw = pose.orientation_as_list() - wxyz = [xyzw[3], *xyzw[:3]] - self.writer.set_body_pose(body_name, pose.position_as_list(), wxyz) + @staticmethod + def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: + return [xyzw[3], *xyzw[:3]] def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 470250e3f..51635addd 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -5,7 +5,8 @@ from typing_extensions import List, Dict, Tuple, Optional from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress -from ..multiverse_datastructures.enums import MultiverseAPIName as API +from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, + MultiverseProperty as Property) from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.pose import Pose from ...datastructures.world import World @@ -77,10 +78,14 @@ def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = param wait: Whether to wait for the data. return: The positions and orientations of the bodies as a dictionary. """ - data = self.get_multiple_body_data(body_names, {name: ["position", "quaternion"] for name in body_names}, + data = self.get_multiple_body_data(body_names, + {name: [BodyProperty.POSITION, BodyProperty.ORIENTATION] + for name in body_names + }, wait=wait) if data is not None: - return {name: Pose(data[name]["position"], self.wxyz_to_xyzw(data[name]["quaternion"])) + return {name: Pose(data[name][BodyProperty.POSITION.value], + self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) for name in body_names} def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: @@ -100,7 +105,7 @@ def get_multiple_body_positions(self, body_names: List[str], param wait: Whether to wait for the data. return: The positions of the bodies as a dictionary. """ - return self.get_multiple_body_properties(body_names, ["position"], wait=wait) + return self.get_multiple_body_properties(body_names, [BodyProperty.POSITION], wait=wait) def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: """ @@ -119,23 +124,23 @@ def get_multiple_body_orientations(self, body_names: List[str], param wait: Whether to wait for the data. return: The orientations of the bodies as a dictionary. """ - data = self.get_multiple_body_properties(body_names, ["quaternion"], wait=wait) + data = self.get_multiple_body_properties(body_names, [BodyProperty.ORIENTATION], wait=wait) if data is not None: - return {name: self.wxyz_to_xyzw(data[name]["quaternion"]) for name in body_names} + return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} - def get_body_property(self, name: str, property_name: str) -> Optional[List[float]]: + def get_body_property(self, name: str, property_: Property) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. - param property_name: The name of the property. + param property_: The property of the body as a Property. param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_name]) + data = self.get_multiple_body_properties([name], [property_]) if data is not None: - return data[name][property_name] + return data[name][property_.value] - def get_multiple_body_properties(self, body_names: List[str], properties: List[str], + def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: """ Get the body properties from the multiverse server for multiple bodies. @@ -169,7 +174,8 @@ def get_body_data(self, name: str, if data is not None: return data[name] - def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[str]]] = None, + def get_multiple_body_data(self, body_names: List[str], + properties: Optional[Dict[str, List[Property]]] = None, wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server for multiple bodies. @@ -187,7 +193,7 @@ def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dic return {name: data[name] for name in body_names} def wait_for_multiple_body_data(self, body_names: List[str], - properties: Optional[Dict[str, List[str]]] = None) -> Dict: + properties: Optional[Dict[str, List[Property]]] = None) -> Dict: """ Wait for the body data from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -208,7 +214,7 @@ def wait_for_multiple_body_data(self, body_names: List[str], raise ValueError(msg) def check_multiple_body_data(self, body_names: List[str], data: Dict, - properties: Optional[Dict[str, List[str]]] = None) -> bool: + properties: Optional[Dict[str, List[Property]]] = None) -> bool: """ Check if the body data is received from the multiverse server for multiple bodies. param body_names: The names of the bodies. @@ -222,7 +228,7 @@ def check_multiple_body_data(self, body_names: List[str], data: Dict, return all([self.check_for_body_data(name, data, properties[name]) for name in body_names]) @staticmethod - def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = None) -> bool: + def check_for_body_data(name: str, data: Dict, properties: Optional[List[Property]] = None) -> bool: """ Check if the body data is received from the multiverse server. param name: The name of the body. @@ -233,7 +239,8 @@ def check_for_body_data(name: str, data: Dict, properties: Optional[List[str]] = if properties is None: return name in data else: - return name in data and all([prop in data[name] and None not in data[name][prop] for prop in properties]) + return name in data and all([prop.value in data[name] and None not in data[name][prop.value] + for prop in properties]) def get_received_data(self): """ @@ -296,10 +303,15 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List param position: The position of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, - {"position": position, - "quaternion": orientation} - ) + self.set_multiple_body_poses({body_name: {BodyProperty.POSITION: position, + BodyProperty.ORIENTATION: orientation}}) + + def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: + """ + Set the body poses in the simulation for multiple bodies. + param body_data: The data to be sent for multiple bodies. + """ + self.send_multiple_body_data_to_server(body_data) def set_body_position(self, body_name: str, position: List[float]) -> None: """ @@ -307,7 +319,7 @@ def set_body_position(self, body_name: str, position: List[float]) -> None: param body_name: The name of the body. param position: The position of the body. """ - self.send_body_data_to_server(body_name, {"position": position}) + self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position}) def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: """ @@ -315,7 +327,7 @@ def set_body_orientation(self, body_name: str, orientation: List[float]) -> None param body_name: The name of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, {"quaternion": orientation}) + self.send_body_data_to_server(body_name, {BodyProperty.ORIENTATION: orientation}) def remove_body(self, body_name: str) -> None: """ @@ -332,24 +344,22 @@ def reset_world(self) -> None: """ self.send_data_to_server([0]) - def send_body_data_to_server(self, body_name: str, data: Dict[str, List[float]]) -> Dict: + def send_body_data_to_server(self, body_name: str, data: Dict[Property, List[float]]) -> Dict: """ Send data to the multiverse server. param body_name: The name of the body. param data: The data to be sent. return: The response from the server. """ - flattened_data = [value for values in data.values() for value in values] - return self.send_data_to_server([time() - self.time_start, *flattened_data], - send_meta_data={body_name: list(data.keys())}) + return self.send_multiple_body_data_to_server({body_name: data}) - def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[float]]]) -> Dict: + def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ Send data to the multiverse server for multiple bodies. param body_data: The data to be sent for multiple bodies. return: The response from the server. """ - send_meta_data = {body_name: list(data.keys()) for body_name, data in body_data.items()} + send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} response_meta_data = self.send_meta_data_and_get_response(send_meta_data) body_names = list(response_meta_data["send"].keys()) flattened_data = [value for body_name in body_names for data in body_data[body_name].values() @@ -358,6 +368,15 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ self.send_and_receive_data() return self.response_meta_data + @staticmethod + def get_actuator_name(body_name: str) -> str: + """ + Get the actuator name from the body name. + param body_name: The name of the body. + return: The actuator name. + """ + return body_name.replace("_joint", "_actuator") + def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: """ Send metadata to the multiverse server and get the response. diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index 7ffd8c8aa..cb001a789 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -1,3 +1,4 @@ +from abc import ABC from enum import Enum @@ -11,3 +12,27 @@ class MultiverseAPIName(Enum): DETACH = "detach" GET_RAYS = "get_rays" EXIST = "exist" + + +class MultiverseProperty(Enum): + def __str__(self): + return self.value + + +class MultiverseBodyProperty(MultiverseProperty): + """ + Enum for the different properties of a body the Multiverse. + """ + POSITION = "position" + ORIENTATION = "quaternion" + RELATIVE_VELOCITY = "relative_velocity" + + +class MultiverseJointProperty(MultiverseProperty): + """ + Enum for the different properties of a joint the Multiverse. + """ + REVOLUTE_JOINT_POSITION = "joint_rvalue" + PRISMATIC_JOINT_POSITION = "joint_tvalue" + REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" + PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index c183cd488..6ebfe04ff 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -164,7 +164,7 @@ def validate_multi_joint_goal(self, goal_validator, joint_types: Optional[List[J for percent in [0.5, 1]: current_joint_positions = goal_joint_positions * percent - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_positions.tolist()))) self.assertTrue(np.allclose(self.robot.get_joint_position('torso_lift_joint'), current_joint_positions[0], atol=0.001)) self.assertTrue( @@ -307,7 +307,7 @@ def validate_list_of_revolute_joint_positions_goal(self, goal_validator, for percent in [0.5, 1]: current_joint_position = goal_joint_positions * percent - self.robot.set_joint_positions(dict(zip(joint_names, current_joint_position))) + self.robot.set_multiple_joint_positions(dict(zip(joint_names, current_joint_position))) self.assertTrue(np.allclose(list(self.robot.get_multiple_joint_positions(joint_names).values()), current_joint_position, atol=0.001)) if percent == 1: From 08bdb4a60eb7e64454700ab5933e90972e315c5f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 25 Jul 2024 19:53:33 +0200 Subject: [PATCH 079/189] [Multiverse] Faster and all tests are running, applied concept of single responsibility. --- src/pycram/datastructures/world.py | 6 +- src/pycram/worlds/bullet_world.py | 14 ++- src/pycram/worlds/multiverse.py | 19 +++- .../multiverse_communication/clients.py | 87 +++++++++++++------ test/test_orm.py | 1 + 5 files changed, 92 insertions(+), 35 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e1ca2f471..f9433780a 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -726,6 +726,7 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError + @abstractmethod def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation @@ -734,7 +735,7 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: :param joint_position: The new joint pose. :return: True if the reset was successful, False otherwise """ - return self.set_multiple_joint_positions({joint: joint_position}) + pass @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: @@ -752,6 +753,7 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass + @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, @@ -761,7 +763,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. """ - return self.reset_multiple_objects_base_poses({obj: pose}) + pass @abstractmethod def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 253776796..171a34445 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -184,7 +184,11 @@ def parse_points_list_to_args(self, point: List) -> Dict: def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): - p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) + self.reset_joint_position(joint, joint_position) + return True + + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: @@ -192,8 +196,12 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: for obj, pose in objects.items(): - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), - physicsClientId=self.id) + self.reset_object_base_pose(obj, pose) + + def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: + p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + physicsClientId=self.id) + return True def step(self): p.stepSimulation(physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 854a080fe..55d67d6dc 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -191,6 +191,10 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) + def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: + self.writer.set_body_property(joint.name, self.get_joint_position_name(joint), [joint_position]) + return True + @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: data = {joint.name: {self.get_joint_position_name(joint): [position]} @@ -199,9 +203,10 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b return True def get_joint_position(self, joint: Joint) -> Optional[float]: - data = self.get_multiple_joint_positions([joint]) + joint_position_name = self.get_joint_position_name(joint) + data = self.reader.get_body_data(joint.name, [joint_position_name]) if data is not None: - return data[joint.name] + return data[joint_position_name.value][0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] @@ -282,7 +287,15 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona :param wait: Whether to wait until the pose is received. return: The pose of the body. """ - return self.reader.get_body_pose(body_name, wait) + data = self.reader.get_body_pose(body_name, wait) + return Pose(data[MultiverseBodyProperty.POSITION.value], + self.wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) + + def wxyz_to_xyzw(self, wxyz: List[float]) -> List[float]: + """ + Convert a quaternion from WXYZ to XYZW format. + """ + return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: return self.reader.get_multiple_body_poses(body_names) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 51635addd..792c977ae 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -2,12 +2,12 @@ import threading from time import time, sleep -from typing_extensions import List, Dict, Tuple, Optional +from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) -from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.pose import Pose from ...datastructures.world import World from ...world_concepts.constraints import Constraint @@ -35,7 +35,6 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: Optional[float] = 2 """ The maximum wait time for the data in seconds. @@ -60,16 +59,14 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = self.thread.start() - def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Pose]: + def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: """ Get the body pose from the multiverse server. param name: The name of the body. param wait: Whether to wait for the data. return: The position and orientation of the body. """ - data = self.get_multiple_body_poses([name], wait=wait) - if data is not None: - return data[name] + return self.get_body_data(name, [BodyProperty.POSITION, BodyProperty.ORIENTATION], wait=wait) def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, Pose]]: """ @@ -95,7 +92,7 @@ def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional param wait: Whether to wait for the data. return: The position of the body. """ - return self.get_multiple_body_positions([name], wait=wait)[name] + return self.get_body_property(name, BodyProperty.POSITION, wait=wait) def get_multiple_body_positions(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: @@ -114,7 +111,7 @@ def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optio param wait: Whether to wait for the data. return: The orientation of the body. """ - return self.get_multiple_body_orientations([name], wait=wait)[name] + return self.get_body_property(name, BodyProperty.ORIENTATION, wait=wait) def get_multiple_body_orientations(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: @@ -128,7 +125,7 @@ def get_multiple_body_orientations(self, body_names: List[str], if data is not None: return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} - def get_body_property(self, name: str, property_: Property) -> Optional[List[float]]: + def get_body_property(self, name: str, property_: Property, wait: Optional[bool] = False) -> Optional[List[float]]: """ Get the body property from the multiverse server. param name: The name of the body. @@ -136,9 +133,9 @@ def get_body_property(self, name: str, property_: Property) -> Optional[List[flo param wait: Whether to wait for the data. return: The property of the body. """ - data = self.get_multiple_body_properties([name], [property_]) + data = self.get_body_data(name, [property_], wait=wait) if data is not None: - return data[name][property_.value] + return data[property_.value] def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: @@ -161,7 +158,8 @@ def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: return quaternion[1:] + [quaternion[0]] def get_body_data(self, name: str, - properties: Optional[List[str]] = None) -> Optional[Dict]: + properties: Optional[List[Property]] = None, + wait: Optional[bool] = False) -> Optional[Dict]: """ Get the body data from the multiverse server. param name: The name of the body. @@ -169,9 +167,11 @@ def get_body_data(self, name: str, param wait: Whether to wait for the data. return: The body data as a dictionary. """ - properties = {name: properties} if properties is not None else None - data = self.get_multiple_body_data([name], properties) - if data is not None: + if wait: + return self.wait_for_body_data(name, properties) + + data = self.get_received_data() + if self.check_for_body_data(name, data, properties): return data[name] def get_multiple_body_data(self, body_names: List[str], @@ -192,6 +192,15 @@ def get_multiple_body_data(self, body_names: List[str], if self.check_multiple_body_data(body_names, data, properties): return {name: data[name] for name in body_names} + def wait_for_body_data(self, name: str, properties: Optional[List[Property]] = None) -> Dict: + """ + Wait for the body data from the multiverse server. + param name: The name of the body. + param properties: The properties of the body. + return: The body data as a dictionary. + """ + return self._wait_for_body_data_template(name, self.check_for_body_data, properties)[name] + def wait_for_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[Property]]] = None) -> Dict: """ @@ -200,16 +209,29 @@ def wait_for_multiple_body_data(self, body_names: List[str], param properties: The properties of the bodies. return: The body data as a dictionary. """ + return self._wait_for_body_data_template(body_names, self.check_multiple_body_data, properties) + + def _wait_for_body_data_template(self, body_names: Union[str, List[str]], + check_func: Callable[[Union[str, List[str]], Dict, Union[Dict, List]], bool], + properties: Optional[Union[Dict, List]] = None) -> Dict: + """ + Wait for the body data from the multiverse server for multiple bodies. + param body_names: The names of the bodies. + param properties: The properties of the bodies. + param check_func: The function to check if the data is received. + param return_func: The function to return the data. + return: The body data as a dictionary. + """ start = time() data_received_flag = False while time() - start < self.MAX_WAIT_TIME_FOR_DATA: received_data = self.get_received_data() - data_received_flag = self.check_multiple_body_data(body_names, received_data, properties) + data_received_flag = check_func(body_names, received_data, properties) if data_received_flag: - return {name: received_data[name] for name in body_names} + return received_data if not data_received_flag: properties_str = "Data" if properties is None else f"Properties {properties}" - msg = f"{properties_str} for bodies {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" + msg = f"{properties_str} for {body_names} not received within {self.MAX_WAIT_TIME_FOR_DATA} seconds" logging.error(msg) raise ValueError(msg) @@ -303,8 +325,9 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List param position: The position of the body. param orientation: The orientation of the body. """ - self.set_multiple_body_poses({body_name: {BodyProperty.POSITION: position, - BodyProperty.ORIENTATION: orientation}}) + self.send_body_data_to_server(body_name, + {BodyProperty.POSITION: position, + BodyProperty.ORIENTATION: orientation}) def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: """ @@ -319,7 +342,7 @@ def set_body_position(self, body_name: str, position: List[float]) -> None: param body_name: The name of the body. param position: The position of the body. """ - self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position}) + self.set_body_property(body_name, BodyProperty.POSITION, position) def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: """ @@ -327,7 +350,16 @@ def set_body_orientation(self, body_name: str, orientation: List[float]) -> None param body_name: The name of the body. param orientation: The orientation of the body. """ - self.send_body_data_to_server(body_name, {BodyProperty.ORIENTATION: orientation}) + self.set_body_property(body_name, BodyProperty.ORIENTATION, orientation) + + def set_body_property(self, body_name: str, property_: Property, value: List[float]) -> None: + """ + Set the body property in the simulation. + param body_name: The name of the body. + param property_: The property of the body. + param value: The value of the property. + """ + self.send_body_data_to_server(body_name, {property_: value}) def remove_body(self, body_name: str) -> None: """ @@ -344,14 +376,16 @@ def reset_world(self) -> None: """ self.send_data_to_server([0]) - def send_body_data_to_server(self, body_name: str, data: Dict[Property, List[float]]) -> Dict: + def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, List[float]]) -> Dict: """ Send data to the multiverse server. param body_name: The name of the body. - param data: The data to be sent. + param body_data: The data to be sent. return: The response from the server. """ - return self.send_multiple_body_data_to_server({body_name: data}) + send_meta_data = {body_name: list(map(str, body_data.keys()))} + flattened_data = [value for data in body_data.values() for value in data] + return self.send_data_to_server([time() - self.time_start, *flattened_data], send_meta_data) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ @@ -410,7 +444,6 @@ def send_data_to_server(self, data: List, class MultiverseAPI(MultiverseClient): - BASE_NAME: str = "api_requester" """ The base name of the Multiverse reader. diff --git a/test/test_orm.py b/test/test_orm.py index 482ce14e3..7f33c65e8 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,4 +1,5 @@ import os +import time import unittest from sqlalchemy import select import sqlalchemy.orm From f1c40574ee4f9d2fcdd2e579cd1470930623f4db Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 26 Jul 2024 11:49:29 +0200 Subject: [PATCH 080/189] [Multiverse] added single joint validator. --- src/pycram/datastructures/world.py | 11 +++++---- src/pycram/worlds/multiverse.py | 4 +++- .../multiverse_functions/goal_validator.py | 23 +++++++++++++++++++ test/test_multiverse.py | 2 +- 4 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index f9433780a..113682211 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -229,16 +229,19 @@ def _init_goal_validators(self): Initializes the goal validators for the World objects' poses, positions, and orientations. """ - # Goal validators for an object + # Objects Pose goal validators self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, self.acceptable_percentage_of_goal) - - # Goal validators for multiple objects self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), self.acceptable_pose_error, self.acceptable_percentage_of_goal) - # Goal validator for the joints of an object + # Joint Goal validators + self.joint_position_goal_validator = JointPositionGoalValidator( + self.get_joint_position, + acceptable_orientation_error=self.acceptable_orientation_error, + acceptable_position_error=self.acceptable_position_error, + acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), acceptable_orientation_error=self.acceptable_orientation_error, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 55d67d6dc..a8bad45d2 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,7 +7,8 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions +from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ + validate_joint_position from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -191,6 +192,7 @@ def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: return self.reader.get_multiple_body_orientations([link.name for link in links]) + @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: self.writer.set_body_property(joint.name, self.get_joint_position_name(joint), [joint_position]) return True diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_functions/goal_validator.py index b59e70168..980faae9b 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_functions/goal_validator.py @@ -446,12 +446,14 @@ def validate_object_pose(pose_setter_func): """ def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): + attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) if len(attachments_pose_goal) == 0: world.pose_goal_validator.register_goal(pose, obj) else: world.multi_pose_goal_validator.register_goal([pose, *list(attachments_pose_goal.values())], [obj, *list(attachments_pose_goal.keys())]) + if not pose_setter_func(world, obj, pose): world.pose_goal_validator.reset() world.multi_pose_goal_validator.reset() @@ -464,6 +466,27 @@ def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): return wrapper +def validate_joint_position(position_setter_func): + """ + A decorator to validate the joint position. + :param position_setter_func: The function to set the joint position. + """ + + def wrapper(world: 'World', joint: 'Joint', position: float): + + joint_type = joint.type + world.joint_position_goal_validator.register_goal(position, joint_type, joint) + + if not position_setter_func(world, joint, position): + world.joint_position_goal_validator.reset() + return False + + world.joint_position_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + def validate_multiple_joint_positions(position_setter_func): """ A decorator to validate the joint positions, this function does not validate the virtual joints, diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 44c6c7084..91bdc52d2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -175,7 +175,7 @@ def test_set_robot_orientation(self): self.multiverse.robot.set_orientation(new_quaternion) robot_orientation = self.multiverse.robot.get_orientation_as_list() quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) - self.assertAlmostEqual(quaternion_difference, 0, delta=0.01) + self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) # self.tearDown() def test_attach_object(self): From acd63d99fc623f478e9134c025c5018ba6d81bfd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 26 Jul 2024 21:07:50 +0200 Subject: [PATCH 081/189] [Multiverse] tests are not running --- src/pycram/datastructures/world.py | 118 +++------- src/pycram/datastructures/world_entity.py | 79 +++++++ src/pycram/description.py | 58 ++++- src/pycram/object_descriptors/urdf.py | 83 ++++--- src/pycram/robot_description.py | 50 ++++- .../robot_descriptions/tiago_description.py | 7 +- src/pycram/world_concepts/world_object.py | 46 +++- src/pycram/worlds/multiverse.py | 148 ++++++------- .../client_manager.py | 12 +- .../multiverse_communication/clients.py | 47 +++- .../worlds/multiverse_communication/socket.py | 208 ++++++++++++++++-- .../__init__.py | 0 .../error_checkers.py | 0 .../goal_validator.py | 4 +- .../worlds/multiverse_extras/helpers.py | 100 +++++++++ test/test_error_checkers.py | 2 +- test/test_goal_validator.py | 4 +- test/test_multiverse.py | 89 ++++++-- test/test_robot_description.py | 6 +- 19 files changed, 779 insertions(+), 282 deletions(-) create mode 100644 src/pycram/datastructures/world_entity.py rename src/pycram/worlds/{multiverse_functions => multiverse_extras}/__init__.py (100%) rename src/pycram/worlds/{multiverse_functions => multiverse_extras}/error_checkers.py (100%) rename src/pycram/worlds/{multiverse_functions => multiverse_extras}/goal_validator.py (99%) create mode 100644 src/pycram/worlds/multiverse_extras/helpers.py diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 113682211..935f4284d 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -18,7 +18,7 @@ MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, - ObjectState, State, WorldState, ClosestPointsList, + ObjectState, WorldState, ClosestPointsList, ContactPointsList, VirtualMoveBaseJoints) from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform @@ -26,88 +26,16 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event -from ..worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ - calculate_quaternion_difference -from ..worlds.multiverse_functions.goal_validator import MultiPoseGoalValidator, \ - MultiPositionGoalValidator, MultiOrientationGoalValidator, PoseGoalValidator, PositionGoalValidator, \ - OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator +from ..worlds.multiverse_extras.goal_validator import (MultiPoseGoalValidator, PoseGoalValidator, + JointPositionGoalValidator, MultiJointPositionGoalValidator, + GoalValidator) +from ..datastructures.world_entity import StateEntity if TYPE_CHECKING: from ..world_concepts.world_object import Object from ..description import Link, Joint -class StateEntity: - """ - The StateEntity class is used to store the state of an object or the physics simulator. This is used to save and - restore the state of the World. - """ - - def __init__(self): - self._saved_states: Dict[int, State] = {} - - @property - def saved_states(self) -> Dict[int, State]: - """ - Returns the saved states of this entity. - """ - return self._saved_states - - def save_state(self, state_id: int) -> int: - """ - Saves the state of this entity with the given state id. - - :param state_id: The unique id of the state. - """ - self._saved_states[state_id] = self.current_state - return state_id - - @property - @abstractmethod - def current_state(self) -> State: - """ - Returns the current state of this entity. - - :return: The current state of this entity. - """ - pass - - @current_state.setter - @abstractmethod - def current_state(self, state: State) -> None: - """ - Sets the current state of this entity. - - :param state: The new state of this entity. - """ - pass - - def restore_state(self, state_id: int) -> None: - """ - Restores the state of this entity from a saved state using the given state id. - - :param state_id: The unique id of the state. - """ - self.current_state = self.saved_states[state_id] - - def remove_saved_states(self) -> None: - """ - Removes all saved states of this entity. - """ - self._saved_states = {} - - -class WorldEntity(StateEntity, ABC): - """ - A data class that represents an entity of the world, such as an object or a link. - """ - - def __init__(self, _id: int, world: Optional[World] = None): - StateEntity.__init__(self) - self.id = _id - self.world: World = world if world is not None else World.current_world - - class World(StateEntity, ABC): """ The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about @@ -161,7 +89,7 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ - acceptable_position_error: float = 5e-3 # 5 cm + acceptable_position_error: float = 5e-2 # 5 cm acceptable_orientation_error: float = 10 * np.pi / 180 # 10 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) use_percentage_of_goal: bool = True @@ -224,6 +152,33 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_goal_validators() + @property + def robot_description(self) -> RobotDescription: + """ + Returns the current robot description. + """ + return RobotDescription.current_robot_description + + @property + def robot_has_actuators(self) -> bool: + """ + Returns whether the robot has actuators. + """ + return self.robot_description.has_actuators + + def get_actuator_for_joint(self, joint: Joint) -> str: + """ + Get the actuator name for a given joint. + """ + return self.robot_joint_actuators.get(joint.name, joint.name) + + @property + def robot_joint_actuators(self) -> Dict[str, str]: + """ + Returns the joint actuators of the robot. + """ + return self.robot_description.joint_actuators + def _init_goal_validators(self): """ Initializes the goal validators for the World objects' poses, positions, and orientations. @@ -648,14 +603,13 @@ def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: move_base_joints.translation_y: position_diff[1], move_base_joints.angular_z: target_angle} - @staticmethod - def get_move_base_joints() -> VirtualMoveBaseJoints: + def get_move_base_joints(self) -> VirtualMoveBaseJoints: """ Get the move base joints of the robot. :return: The move base joints. """ - return RobotDescription.current_robot_description.virtual_move_base_joints + return self.robot_description.virtual_move_base_joints @staticmethod def get_position_diff(current_position: List[float], target_position: List[float]) -> List[float]: @@ -793,7 +747,7 @@ def get_arm_tool_frame_link(self, arm: Arms) -> Link: :param arm: The arm for which the tool frame link should be returned. :return: The tool frame link of the arm. """ - ee_link_name = RobotDescription.current_robot_description.get_arm_tool_frame(arm) + ee_link_name = self.robot_description.get_arm_tool_frame(arm) return self.robot.get_link(ee_link_name) @abstractmethod diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py new file mode 100644 index 000000000..c21ae782a --- /dev/null +++ b/src/pycram/datastructures/world_entity.py @@ -0,0 +1,79 @@ +from abc import ABC, abstractmethod + +from typing_extensions import Optional, TYPE_CHECKING, Dict + +from .dataclasses import State + +if TYPE_CHECKING: + from ..datastructures.world import World + + +class StateEntity: + """ + The StateEntity class is used to store the state of an object or the physics simulator. This is used to save and + restore the state of the World. + """ + + def __init__(self): + self._saved_states: Dict[int, State] = {} + + @property + def saved_states(self) -> Dict[int, State]: + """ + Returns the saved states of this entity. + """ + return self._saved_states + + def save_state(self, state_id: int) -> int: + """ + Saves the state of this entity with the given state id. + + :param state_id: The unique id of the state. + """ + self._saved_states[state_id] = self.current_state + return state_id + + @property + @abstractmethod + def current_state(self) -> State: + """ + Returns the current state of this entity. + + :return: The current state of this entity. + """ + pass + + @current_state.setter + @abstractmethod + def current_state(self, state: State) -> None: + """ + Sets the current state of this entity. + + :param state: The new state of this entity. + """ + pass + + def restore_state(self, state_id: int) -> None: + """ + Restores the state of this entity from a saved state using the given state id. + + :param state_id: The unique id of the state. + """ + self.current_state = self.saved_states[state_id] + + def remove_saved_states(self) -> None: + """ + Removes all saved states of this entity. + """ + self._saved_states = {} + + +class WorldEntity(StateEntity, ABC): + """ + A data class that represents an entity of the world, such as an object or a link. + """ + + def __init__(self, _id: int, world: 'World'): + StateEntity.__init__(self) + self.id = _id + self.world: 'World' = world diff --git a/src/pycram/description.py b/src/pycram/description.py index 83f796ac2..2f63c6882 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -11,7 +11,7 @@ from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform -from .datastructures.world import WorldEntity +from .datastructures.world_entity import WorldEntity from .local_transformer import LocalTransformer if TYPE_CHECKING: @@ -120,7 +120,7 @@ def upper_limit(self) -> Union[float, None]: @property @abstractmethod - def parent_link_name(self) -> str: + def parent(self) -> str: """ :return: The name of the parent link of this joint. """ @@ -128,7 +128,7 @@ def parent_link_name(self) -> str: @property @abstractmethod - def child_link_name(self) -> str: + def child(self) -> str: """ :return: The name of the child link of this joint. """ @@ -504,7 +504,7 @@ def parent_link(self) -> Link: :return: The parent link as a AbstractLink object. """ - return self.object.get_link(self.parent_link_name) + return self.object.get_link(self.parent) @property def child_link(self) -> Link: @@ -513,7 +513,7 @@ def child_link(self) -> Link: :return: The child link as a AbstractLink object. """ - return self.object.get_link(self.child_link_name) + return self.object.get_link(self.child) @property def position(self) -> float: @@ -612,6 +612,12 @@ def __init__(self, path: Optional[str] = None): """ :param path: The path of the file to update the description data from. """ + + self._links: Optional[List[LinkDescription]] = None + self._joints: Optional[List[JointDescription]] = None + self._link_map: Optional[Dict[str, Any]] = None + self._joint_map: Optional[Dict[str, Any]] = None + if path: self.update_description_from_file(path) else: @@ -619,6 +625,39 @@ def __init__(self, path: Optional[str] = None): self.virtual_joint_names: List[str] = [] + @property + @abstractmethod + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + pass + + @property + @abstractmethod + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + pass + + @property + @abstractmethod + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + pass + + @property + @abstractmethod + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + pass + def is_joint_virtual(self, name: str) -> bool: """ Returns whether the joint with the given name is virtual. @@ -770,12 +809,11 @@ def links(self) -> List[LinkDescription]: """ pass - @abstractmethod def get_link_by_name(self, link_name: str) -> LinkDescription: """ :return: The link description with the given name. """ - pass + return self.link_map[link_name] @property @abstractmethod @@ -785,12 +823,11 @@ def joints(self) -> List[JointDescription]: """ pass - @abstractmethod def get_joint_by_name(self, joint_name: str) -> JointDescription: """ :return: The joint description with the given name. """ - pass + return self.joint_map[joint_name] @abstractmethod def get_root(self) -> str: @@ -806,7 +843,8 @@ def get_tip(self) -> str: raise NotImplementedError @abstractmethod - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: """ :return: the chain of links from 'start_link_name' to 'end_link_name'. """ diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index c5b51fb60..44d8f1489 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -5,7 +5,7 @@ import rospy from geometry_msgs.msg import Point from tf.transformations import quaternion_from_euler, euler_from_quaternion -from typing_extensions import Union, List, Optional +from typing_extensions import Union, List, Optional, Dict, Tuple from urdf_parser_py import urdf from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder, Sphere as URDF_Sphere, Mesh as URDF_Mesh) @@ -132,14 +132,14 @@ def upper_limit(self) -> Union[float, None]: return None @property - def parent_link_name(self) -> str: + def parent(self) -> str: """ :return: The name of the parent link of this joint. """ return self.parsed_description.parent @property - def child_link_name(self) -> str: + def child(self) -> str: """ :return: The name of the child link of this joint. """ @@ -174,6 +174,39 @@ class RootLink(AbstractObjectDescription.RootLink, Link): class Joint(AbstractObjectDescription.Joint, JointDescription): ... + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + return self.parsed_description.child_map + + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + return self.parsed_description.parent_map + + @property + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + if self._link_map is None: + self._link_map = {link.name: link for link in self.links} + return self._link_map + + @property + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + if self._joint_map is None: + self._joint_map = {joint.name: joint for joint in self.joints} + return self._joint_map + def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, @@ -256,37 +289,23 @@ def generate_from_parameter_server(self, name: str) -> str: urdf_string = rospy.get_param(name) return self.correct_urdf_string(urdf_string) - def get_link_by_name(self, link_name: str) -> LinkDescription: - """ - :return: The link description with the given name. - """ - for link in self.links: - if link.name == link_name: - return link - raise ValueError(f"Link with name {link_name} not found") - @property - def links(self) -> List[LinkDescription]: - """ - :return: A list of links descriptions of this object. - """ - return [LinkDescription(link) for link in self.parsed_description.links] - - def get_joint_by_name(self, joint_name: str) -> JointDescription: + def joints(self) -> List[JointDescription]: """ - :return: The joint description with the given name. + :return: A list of joints descriptions of this object. """ - for joint in self.joints: - if joint.name == joint_name: - return joint - raise ValueError(f"Joint with name {joint_name} not found") + if self._joints is None: + self._joints = [JointDescription(joint) for joint in self.parsed_description.joints] + return self._joints @property - def joints(self) -> List[JointDescription]: + def links(self) -> List[LinkDescription]: """ - :return: A list of joints descriptions of this object. + :return: A list of link descriptions of this object. """ - return [JointDescription(joint) for joint in self.parsed_description.joints] + if self._links is None: + self._links = [LinkDescription(link) for link in self.parsed_description.links] + return self._links def get_root(self) -> str: """ @@ -306,11 +325,17 @@ def get_tip(self) -> str: link = child return link - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: """ + :param start_link_name: The name of the start link of the chain. + :param end_link_name: The name of the end link of the chain. + :param joints: Whether to include joints in the chain. + :param links: Whether to include links in the chain. + :param fixed: Whether to include fixed joints in the chain. :return: the chain of links from 'start_link_name' to 'end_link_name'. """ - return self.parsed_description.get_chain(start_link_name, end_link_name) + return self.parsed_description.get_chain(start_link_name, end_link_name, joints, links, fixed) def correct_urdf_string(self, urdf_string: str) -> str: """ diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index c6b77a67b..e6af1aaad 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -3,11 +3,12 @@ import rospy from typing_extensions import List, Dict, Union, Optional -from urdf_parser_py.urdf import URDF from .datastructures.dataclasses import VirtualMoveBaseJoints +from .datastructures.enums import Arms, Grasp, GripperState, GripperType, JointType +from .object_descriptors.urdf import ObjectDescription as URDFObject from .utils import suppress_stdout_stderr -from .datastructures.enums import Arms, Grasp, GripperState, GripperType +from .worlds.multiverse_extras.helpers import parse_mjcf_actuators class RobotDescriptionManager: @@ -82,7 +83,7 @@ class RobotDescription: """ Torso joint of the robot """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -113,7 +114,7 @@ class RobotDescription: """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None): + virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None, mjcf_path: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -123,6 +124,8 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param torso_link: Torso link of the robot :param torso_joint: Torso joint of the robot, this is the joint that moves the torso upwards if there is one :param urdf_path: Path to the URDF file of the robot + :param virtual_move_base_joints: Virtual move base joint names for mobile robots + :param mjcf_path: Path to the MJCF file of the robot """ self.name = name self.base_link = base_link @@ -130,7 +133,9 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.torso_joint = torso_joint with suppress_stdout_stderr(): # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them - self.urdf_object = URDF.from_xml_file(urdf_path) + self.urdf_object = URDFObject(urdf_path) + self.joint_types = {joint.name: joint.type for joint in self.urdf_object.joints} + self.joint_actuators: Optional[Dict] = parse_mjcf_actuators(mjcf_path) if mjcf_path is not None else None self.kinematic_chains: Dict[str, KinematicChainDescription] = {} self.cameras: Dict[str, CameraDescription] = {} self.grasps: Dict[Grasp, List[float]] = {} @@ -138,6 +143,26 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.joints: List[str] = [j.name for j in self.urdf_object.joints] self.virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = virtual_move_base_joints + @property + def has_actuators(self): + """ + Property to check if the robot has actuators defined in the MJCF file. + + :return: True if the robot has actuators, False otherwise + """ + return self.joint_actuators is not None + + def get_actuator_for_joint(self, joint: str) -> Optional[str]: + """ + Returns the actuator name for a given joint. + + :param joint: Name of the joint + :return: Name of the actuator + """ + if self.has_actuators: + return self.joint_actuators.get(joint) + return None + def add_kinematic_chain_description(self, chain: KinematicChainDescription): """ Adds a KinematicChainDescription object to the RobotDescription. The chain is stored with the name of the chain @@ -347,7 +372,7 @@ class KinematicChainDescription: """ Last link of the chain """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -376,7 +401,7 @@ class KinematicChainDescription: Dictionary of static joint states for the chain """ - def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDF, arm_type: Arms = None, + def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDFObject, arm_type: Arms = None, include_fixed_joints=False): """ Initialize the KinematicChainDescription object. @@ -391,7 +416,7 @@ def __init__(self, name: str, start_link: str, end_link: str, urdf_object: URDF, self.name: str = name self.start_link: str = start_link self.end_link: str = end_link - self.urdf_object: URDF = urdf_object + self.urdf_object: URDFObject = urdf_object self.include_fixed_joints: bool = include_fixed_joints self.link_names: List[str] = [] self.joint_names: List[str] = [] @@ -413,7 +438,8 @@ def _init_joints(self): Initializes the joints of the chain by getting the chain from the URDF object. """ joints = self.urdf_object.get_chain(self.start_link, self.end_link, links=False) - self.joint_names = list(filter(lambda j: self.urdf_object.joint_map[j].type != "fixed" or self.include_fixed_joints, joints)) + self.joint_names = list(filter(lambda j: self.urdf_object.joint_map[j].type != JointType.FIXED + or self.include_fixed_joints, joints)) def get_joints(self) -> List[str]: """ @@ -570,7 +596,7 @@ class EndEffectorDescription: """ Name of the tool frame link in the URDf """ - urdf_object: URDF + urdf_object: URDFObject """ Parsed URDF of the robot """ @@ -595,7 +621,7 @@ class EndEffectorDescription: Distance the gripper can open, in cm """ - def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDF): + def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URDFObject): """ Initialize the EndEffectorDescription object. @@ -607,7 +633,7 @@ def __init__(self, name: str, start_link: str, tool_frame: str, urdf_object: URD self.name: str = name self.start_link: str = start_link self.tool_frame: str = tool_frame - self.urdf_object: URDF = urdf_object + self.urdf_object: URDFObject = urdf_object self.link_names: List[str] = [] self.joint_names: List[str] = [] self.static_joint_states: Dict[GripperState, Dict[str, float]] = {} diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index c5c7814d9..d190cd4c4 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -4,12 +4,17 @@ from ..datastructures.enums import GripperState, Arms, Grasp from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription +from ..worlds.multiverse_extras.helpers import get_robot_mjcf_path rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' +mjcf_filename = get_robot_mjcf_path("pal_robotics", "tiago_dual") + tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", - filename, VirtualMoveBaseJoints()) + filename, + virtual_move_base_joints=VirtualMoveBaseJoints(), + mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "torso_lift_link", "arm_left_7_link", diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index ece5eda96..76f398239 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -13,12 +13,13 @@ ContactPointsList) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform -from ..datastructures.world import WorldEntity, World +from ..datastructures.world import World from ..description import ObjectDescription, LinkDescription, Joint from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment +from ..datastructures.world_entity import WorldEntity Link = ObjectDescription.Link @@ -56,7 +57,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ - super().__init__(-1, world) + super().__init__(-1, world if world is not None else World.current_world) if pose is None: pose = Pose() @@ -76,12 +77,15 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose + if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: + self._update_world_robot_and_description() + self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) self.description.update_description_from_file(self.path) if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - self._update_world_robot_and_description() + self._add_virtual_move_base_joints() self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name @@ -95,6 +99,37 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + @property + def joint_actuators(self) -> Optional[Dict[str, str]]: + """ + Returns the joint actuators of the robot. + """ + if self.obj_type == ObjectType.ROBOT: + return self.robot_description.joint_actuators + return None + + @property + def has_actuators(self) -> bool: + """ + Returns True if the object has actuators, otherwise False. + """ + return self.robot_description.has_actuators + + @property + def robot_description(self) -> RobotDescription: + """ + Returns the current robot description. + """ + return self.world.robot_description + + def get_actuator_for_joint(self, joint: Joint) -> Optional[str]: + """ + Get the actuator name for a joint. + :param joint: The joint object for which to get the actuator. + :return: The name of the actuator. + """ + return self.robot_description.get_actuator_for_joint(joint.name) + def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: """ Get the positions of multiple links of the object. @@ -191,15 +226,14 @@ def _load_object_and_get_id(self, path: str, def _update_world_robot_and_description(self): """ Initializes the robot description of the object, loads the description from the RobotDescriptionManager and sets - the robot as the current robot in the World. Also adds the virtual move base joints to the robot. + the robot as the current robot in the World. """ rdm = RobotDescriptionManager() rdm.load_description(self.name) World.robot = self - self._add_virtual_move_base_joints() def _add_virtual_move_base_joints(self): - virtual_joints = RobotDescription.current_robot_description.virtual_move_base_joints + virtual_joints = self.robot_description.virtual_move_base_joints if virtual_joints is None: return child_link = self.root_link_name diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a8bad45d2..2f025ab53 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -6,9 +6,11 @@ from typing_extensions import List, Dict, Optional from .multiverse_communication.client_manager import MultiverseClientManager +from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ +from .multiverse_extras.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position +from .multiverse_extras.helpers import find_multiverse_resources_path from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -26,10 +28,18 @@ class Multiverse(World): _joint_type_to_position_name: Dict[JointType, MultiverseJointProperty] = { JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_POSITION, - JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION, + JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION } """ - A dictionary to map JointType to the corresponding multiverse attribute name. + A dictionary to map JointType to the corresponding multiverse joint position attribute name. + """ + + _joint_type_to_cmd_name: Dict[JointType, MultiverseJointProperty] = { + JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_CMD, + JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_CMD, + } + """ + A dictionary to map JointType to the corresponding multiverse joint command attribute name. """ added_multiverse_resources: bool = False @@ -101,10 +111,14 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, def _init_clients(self): client_manager = MultiverseClientManager(self.simulation_wait_time_factor) - self.reader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) - self.writer = client_manager.create_writer(self.simulation, is_prospection_world=self.is_prospection_world) - self.api_requester = client_manager.create_api_requester(self.simulation, - is_prospection_world=self.is_prospection_world) + self.reader: MultiverseReader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) + self.writer: MultiverseWriter = client_manager.create_writer(self.simulation, + is_prospection_world=self.is_prospection_world) + self.api_requester: MultiverseAPI = client_manager.create_api_requester( + self.simulation, + is_prospection_world=self.is_prospection_world) + # self.joint_controller: MultiverseController = client_manager.create_controller( + # is_prospection_world=self.is_prospection_world) def _set_world_job_flags(self): self.let_pycram_move_attached_objects = False @@ -139,10 +153,27 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def get_joint_position_name(self, joint: Joint) -> MultiverseJointProperty: - if joint.type not in self._joint_type_to_position_name: - raise ValueError(f"Joint type {joint.type} is not supported in Multiverse") - return self._joint_type_to_position_name[joint.type] + def spawn_robot(self, name: str, pose: Pose) -> None: + """ + Spawn the robot in the simulator. + param robot_description: The robot description. + param pose: The pose of the robot. + return: The object of the robot. + """ + # actuator_joint_commands = { + # actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] + # for joint_name, actuator_name in self.robot_joint_actuators.items() + # } + # self.joint_controller.init_controller(actuator_joint_commands) + self.writer.spawn_robot_with_actuators(name, pose.position_as_list(), + self.xyzw_to_wxyz(pose.orientation_as_list())) + # actuator_joint_commands) + + def get_joint_position_name(self, joint_type: JointType) -> MultiverseJointProperty: + return self._joint_type_to_position_name[joint_type] + + def get_joint_cmd_name(self, joint_type: JointType) -> MultiverseJointProperty: + return self._joint_type_to_cmd_name.get(joint_type, self.get_joint_position_name(joint_type)) def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, @@ -157,11 +188,25 @@ def load_object_and_get_id(self, name: Optional[str] = None, if pose is None: pose = Pose() + # Do not spawn objects with type environment as they should be already present in the simulator through the + # multiverse description file (.muv file). if not obj_type == ObjectType.ENVIRONMENT: - self._set_body_pose(name, pose) + self.spawn_object(name, obj_type, pose) return self._update_object_id_name_maps_and_get_latest_id(name) + def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> int: + """ + Spawn the object in the simulator and return the object id. + param obj: The object to be spawned. + param pose: The pose of the object. + return: The object id. + """ + if object_type == ObjectType.ROBOT: + self.spawn_robot(name, pose) + else: + self._set_body_pose(name, pose) + def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: """ Update the object id name maps and return the latest object id. @@ -194,25 +239,31 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - self.writer.set_body_property(joint.name, self.get_joint_position_name(joint), [joint_position]) + self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), + [joint_position]) + # self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), self.get_joint_cmd_name(joint.type), + # [joint_position]) return True @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: - data = {joint.name: {self.get_joint_position_name(joint): [position]} + data = {joint.name: {self.get_joint_position_name(joint.type): [position]} for joint, position in joint_positions.items()} self.writer.send_multiple_body_data_to_server(data) + # data = {self.get_actuator_for_joint(joint): {self.get_joint_cmd_name(joint.type): [position]} + # for joint, position in joint_positions.items()} + # self.joint_controller.send_multiple_body_data_to_server(data) return True def get_joint_position(self, joint: Joint) -> Optional[float]: - joint_position_name = self.get_joint_position_name(joint) + joint_position_name = self.get_joint_position_name(joint.type) data = self.reader.get_body_data(joint.name, [joint_position_name]) if data is not None: return data[joint_position_name.value][0] def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str, float]]: joint_names = [joint.name for joint in joints] - data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint)] + data = self.reader.get_multiple_body_data(joint_names, {joint.name: [self.get_joint_position_name(joint.type)] for joint in joints}) if data is not None: return {name: list(value.values())[0][0] for name, value in data.items()} @@ -293,7 +344,8 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona return Pose(data[MultiverseBodyProperty.POSITION.value], self.wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) - def wxyz_to_xyzw(self, wxyz: List[float]) -> List[float]: + @staticmethod + def wxyz_to_xyzw(wxyz: List[float]) -> List[float]: """ Convert a quaternion from WXYZ to XYZW format. """ @@ -473,65 +525,3 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) - - -def get_resource_paths(dirname: str) -> List[str]: - resources_paths = ["../robots", "../worlds", "../objects"] - resources_paths = [ - os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( - resources_path) else resources_path - for resources_path in resources_paths - ] - - def add_directories(path: str) -> None: - with os.scandir(path) as entries: - for entry in entries: - if entry.is_dir(): - resources_paths.append(entry.path) - add_directories(entry.path) - - resources_path_copy = resources_paths.copy() - for resources_path in resources_path_copy: - add_directories(resources_path) - - return resources_paths - - -def find_multiverse_resources_path() -> Optional[str]: - """ - Find the path to the Multiverse resources directory. - """ - # Get the path to the Multiverse installation - multiverse_path = find_multiverse_path() - - # Check if the path to the Multiverse installation was found - if multiverse_path: - # Construct the path to the resources directory - resources_path = os.path.join(multiverse_path, 'resources') - - # Check if the resources directory exists - if os.path.exists(resources_path): - return resources_path - - return None - - -def find_multiverse_path() -> Optional[str]: - """ - Find the path to the Multiverse installation. - """ - # Get the value of PYTHONPATH environment variable - pythonpath = os.getenv('PYTHONPATH') - - # Check if PYTHONPATH is set - if pythonpath: - # Split the PYTHONPATH into individual paths using the platform-specific path separator - paths = pythonpath.split(os.pathsep) - - # Iterate through each path and check if 'Multiverse' is in it - for path in paths: - if 'multiverse' in path: - multiverse_path = path.split('multiverse')[0] - return multiverse_path + 'multiverse' - - return None diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 750213ca7..896b06745 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -1,7 +1,7 @@ from typing_extensions import Optional, Type, Union, List, Dict from pycram.worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ - MultiverseReader + MultiverseReader, MultiverseController class MultiverseClientManager: @@ -40,6 +40,14 @@ def create_writer(self, simulation: str, is_prospection_world: Optional[bool] = return self.create_client(MultiverseWriter, "writer", is_prospection_world, simulation=simulation) + def create_controller(self, is_prospection_world: Optional[bool] = False) -> MultiverseController: + """ + Create a Multiverse controller client. + param simulation: The name of the simulation that the controller is connected to. + param is_prospection_world: Whether the controller is connected to the prospection world. + """ + return self.create_client(MultiverseController, "controller", is_prospection_world) + def create_api_requester(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseAPI: """ Create a Multiverse API client. @@ -54,7 +62,7 @@ def create_client(self, name: Optional[str] = None, is_prospection_world: Optional[bool] = False, **kwargs) -> Union[MultiverseClient, MultiverseAPI, - MultiverseReader, MultiverseWriter]: + MultiverseReader, MultiverseWriter, MultiverseController]: """ Create a Multiverse client. param kwargs: The keyword arguments. diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 792c977ae..5c57af2f8 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -4,7 +4,7 @@ from typing_extensions import List, Dict, Tuple, Optional, Callable, Union -from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from .socket import MultiverseClient as MultiverseSocket, MultiverseMetaData, SocketAddress from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) @@ -291,7 +291,7 @@ def join(self): class MultiverseWriter(MultiverseClient): - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + def __init__(self, name: str, port: int, simulation: Optional[str] = None, is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. @@ -305,7 +305,19 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation - self.time_start = time() + + def spawn_robot_with_actuators(self, robot_name: str, position: List[float], orientation: List[float], + actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None: + """ + Spawn the robot with controlled actuators in the simulation. + :param robot_name: The name of the robot. + :param position: The position of the robot. + :param orientation: The orientation of the robot. + :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. + """ + send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value]} + data = [self.sim_time, *position, *orientation] + self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands) def _reset_request_meta_data(self): """ @@ -316,7 +328,8 @@ def _reset_request_meta_data(self): "send": {}, "receive": {}, } - self.request_meta_data["meta_data"]["simulation_name"] = self.simulation + if self.simulation is not None: + self.request_meta_data["meta_data"]["simulation_name"] = self.simulation def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]) -> None: """ @@ -366,7 +379,7 @@ def remove_body(self, body_name: str) -> None: Remove the body from the simulation. param body_name: The name of the body. """ - self.send_data_to_server([time() - self.time_start], + self.send_data_to_server([self.sim_time], send_meta_data={body_name: []}, receive_meta_data={body_name: []}) @@ -385,7 +398,7 @@ def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, Lis """ send_meta_data = {body_name: list(map(str, body_data.keys()))} flattened_data = [value for data in body_data.values() for value in data] - return self.send_data_to_server([time() - self.time_start, *flattened_data], send_meta_data) + return self.send_data_to_server([self.sim_time, *flattened_data], send_meta_data) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ @@ -398,7 +411,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, body_names = list(response_meta_data["send"].keys()) flattened_data = [value for body_name in body_names for data in body_data[body_name].values() for value in data] - self.send_data = [time() - self.time_start, *flattened_data] + self.send_data = [self.sim_time, *flattened_data] self.send_and_receive_data() return self.response_meta_data @@ -443,6 +456,26 @@ def send_data_to_server(self, data: List, return self.response_meta_data +class MultiverseController(MultiverseWriter): + + def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + """ + Initialize the Multiverse controller, which controls the robot in the simulation. + This class provides methods to send controller data to the Multiverse server. + param port: The port of the Multiverse controller client. + param is_prospection_world: Whether the controller is connected to the prospection world. + """ + super().__init__(name, port, is_prospection_world=is_prospection_world) + + def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None: + """ + Initialize the controller by sending the controller data to the multiverse server. + :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. + """ + self.send_data_to_server([self.sim_time] + [0.0]*len(actuator_joint_commands), + send_meta_data={key: value for key, value in actuator_joint_commands.items()}) + + class MultiverseAPI(MultiverseClient): BASE_NAME: str = "api_requester" """ diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 7353e95d6..0b36eac0e 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -1,17 +1,18 @@ #!/usr/bin/env python3 +"""Multiverse Client base class.""" + import dataclasses -from typing import List, Dict, TypeVar -import logging +from typing import List, Dict, Callable, TypeVar from multiverse_client_pybind import MultiverseClientPybind # noqa -from typing_extensions import Optional T = TypeVar("T") @dataclasses.dataclass class MultiverseMetaData: + """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" world_name: str = "world" simulation_name: str = "cram" length_unit: str = "m" @@ -22,6 +23,7 @@ class MultiverseMetaData: class SocketAddress: + """Socket address for the Multiverse Server and the Multiverse Client""" host: str = "tcp://127.0.0.1" port: str = "" @@ -29,20 +31,33 @@ def __init__(self, port: str) -> None: self.port = port -class MultiverseSocket: +class MultiverseClient: + """Base class for the Multiverse Client""" _server_addr: SocketAddress = SocketAddress(port="7000") + _client_addr: SocketAddress + _meta_data: MultiverseMetaData + _multiverse_socket: MultiverseClientPybind + _start_time: float + _api_callbacks: Dict[str, Callable[[List[str]], List[str]]] def __init__( self, client_addr: SocketAddress, - multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(), + multiverse_meta_data: MultiverseMetaData, ) -> None: + """ + + Args: + client_addr (SocketAddress): The address of the client. + multiverse_meta_data (MultiverseMetaData): The meta data for the Multiverse Client. + """ if not isinstance(client_addr.port, str) or client_addr.port == "": - raise ValueError(f"Must specify client port for {self.__class__.__name__}") + raise ValueError(f"Must specify client port for {self.__class__.__name__}.") + if multiverse_meta_data.simulation_name == "": + raise ValueError(f"Must specify simulation name.") self._send_data = None self._client_addr = client_addr self._meta_data = multiverse_meta_data - self.client_name = self._meta_data.simulation_name self._multiverse_socket = MultiverseClientPybind( f"{self._server_addr.host}:{self._server_addr.port}" ) @@ -51,65 +66,214 @@ def __init__( "send": {}, "receive": {}, } + self._start_time = 0.0 + + def loginfo(self, message: str) -> None: + """Log information. + + Args: + message (str): The message to log. + + Returns: + None + """ + print(message) + + def logwarn(self, message: str) -> None: + """Warn the user. + + Args: + message (str): The message to warn about. + + Returns: + None + """ + print(message) def run(self) -> None: - message = f"[Client {self._client_addr.port}] Start {self.__class__.__name__}{self._client_addr.port}" - logging.info(message) + """Run the client. + + Returns: + None + """ + message = f"[Client {self._client_addr.port}] Start {self.__class__.__name__}{self._client_addr.port}." + self.loginfo(message) + self._run() + + def _run(self) -> None: + """Run the client, should call the _connect_and_start() method. It's left to the user to implement this method + in threaded or non-threaded fashion. + + Returns: + None + """ self._connect_and_start() def stop(self) -> None: - self._disconnect() + """Stop the client. - def send_and_receive_meta_data(self): - self._communicate(True) - - def send_and_receive_data(self): - self._communicate(False) + Returns: + None + """ + self._disconnect() @property def request_meta_data(self) -> Dict: + """Get the request meta data.""" return self._request_meta_data @request_meta_data.setter def request_meta_data(self, request_meta_data: Dict) -> None: + """Set the request_meta_data, make sure to clear the `send` and `receive` field before setting the request""" self._request_meta_data = request_meta_data self._multiverse_socket.set_request_meta_data(self._request_meta_data) @property def response_meta_data(self) -> Dict: + """Get the response_meta_data.""" response_meta_data = self._multiverse_socket.get_response_meta_data() - if not response_meta_data: + assert isinstance(response_meta_data, dict) + if response_meta_data == {}: message = f"[Client {self._client_addr.port}] Receive empty response meta data." - logging.warning(message) + self.logwarn(message) return response_meta_data + def send_and_receive_meta_data(self): + self._communicate(True) + + def send_and_receive_data(self): + self._communicate(False) + @property def send_data(self) -> List[float]: + """Get the send_data.""" return self._send_data @send_data.setter def send_data(self, send_data: List[float]) -> None: + """Set the send_data, the first element should be the current simulation time, + the rest should be the data to send with the following order: + double -> uint8_t -> uint16_t""" + assert isinstance(send_data, list) self._send_data = send_data self._multiverse_socket.set_send_data(self._send_data) @property def receive_data(self) -> List[float]: + """Get the receive_data, the first element should be the current simulation time, + the rest should be the received data with the following order: + double -> uint8_t -> uint16_t""" receive_data = self._multiverse_socket.get_receive_data() - if not receive_data: - message = f"[Client {self._client_addr.port}] Receive empty data." - logging.warning(message) + assert isinstance(receive_data, list) return receive_data + @property + def api_callbacks(self) -> Dict[str, Callable[[List[str]], List[str]]]: + """Get the api_callbacks.""" + return self._api_callbacks + + @api_callbacks.setter + def api_callbacks(self, api_callbacks: Dict[str, Callable[[List[str]], List[str]]]) -> None: + """Set the api_callbacks.""" + self._multiverse_socket.set_api_callbacks(api_callbacks) + self._api_callbacks = api_callbacks + + def _bind_request_meta_data(self, request_meta_data: T) -> T: + """Bind the request_meta_data before sending it to the server. + + Args: + request_meta_data (T): The request_meta_data to bind. + + Returns: + T: The binded request_meta_data. + """ + pass + + def _bind_response_meta_data(self, response_meta_data: T) -> T: + """Bind the response_meta_data after receiving it from the server. + + Args: + response_meta_data (T): The response_meta_data to bind. + + Returns: + T: The binded response_meta_data. + """ + pass + + def _bind_send_data(self, send_data: T) -> T: + """Bind the send_data before sending it to the server. + + Args: + send_data (T): The send_data to bind. + + Returns: + T: The binded send_data. + """ + pass + + def _bind_receive_data(self, receive_data: T) -> T: + """Bind the receive_data after receiving it from the server. + + Args: + receive_data (T): The receive_data to bind. + + Returns: + T: The binded receive_data. + """ + pass + def _connect_and_start(self) -> None: + """Connect to the server and start the client. + + Returns: + None + """ self._multiverse_socket.connect(self._client_addr.host, self._client_addr.port) self._multiverse_socket.start() + self._start_time = self._multiverse_socket.get_time_now() def _disconnect(self) -> None: + """Disconnect from the server. + + Returns: + None + """ self._multiverse_socket.disconnect() - def _communicate(self, resend_request_meta_data: bool = False) -> None: - self._multiverse_socket.communicate(resend_request_meta_data) + def _communicate(self, resend_request_meta_data: bool = False) -> bool: + """Communicate with the server. + + Args: + resend_request_meta_data (bool): Resend the request meta data. + + Returns: + bool: True if the communication was successful, False otherwise. + """ + return self._multiverse_socket.communicate(resend_request_meta_data) def _restart(self) -> None: + """Restart the client. + + Returns: + None + """ self._disconnect() self._connect_and_start() + + @property + def world_time(self) -> float: + """Get the world time from the server. + + Returns: + float: The world time. + """ + return self._multiverse_socket.get_world_time() + + @property + def sim_time(self) -> float: + """Get the current simulation time. + + Returns: + float: The current simulation time. + """ + return self._multiverse_socket.get_time_now() - self._start_time diff --git a/src/pycram/worlds/multiverse_functions/__init__.py b/src/pycram/worlds/multiverse_extras/__init__.py similarity index 100% rename from src/pycram/worlds/multiverse_functions/__init__.py rename to src/pycram/worlds/multiverse_extras/__init__.py diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/worlds/multiverse_extras/error_checkers.py similarity index 100% rename from src/pycram/worlds/multiverse_functions/error_checkers.py rename to src/pycram/worlds/multiverse_extras/error_checkers.py diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/worlds/multiverse_extras/goal_validator.py similarity index 99% rename from src/pycram/worlds/multiverse_functions/goal_validator.py rename to src/pycram/worlds/multiverse_extras/goal_validator.py index 980faae9b..de8431f7b 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/worlds/multiverse_extras/goal_validator.py @@ -5,7 +5,7 @@ from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ +from pycram.worlds.multiverse_extras.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker if TYPE_CHECKING: @@ -65,7 +65,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, self.register_goal(goal_value, current_value_getter_input, initial_value, acceptable_error) self.wait_until_goal_is_achieved(max_wait_time, time_per_read) - def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 1, + def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, time_per_read: Optional[float] = 0.01) -> None: """ Wait until the target is reached. diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py new file mode 100644 index 000000000..3d2e0284d --- /dev/null +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -0,0 +1,100 @@ +import os + +from typing_extensions import Optional, List, Dict +import xml.etree.ElementTree as ET + + +def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: + """ + Parse the actuator elements from an MJCF file. + :param file_path: The path to the MJCF file. + """ + tree = ET.parse(file_path) + root = tree.getroot() + + joint_actuators = {} + + # Iterate through all actuator elements + for actuator in root.findall(".//actuator/*"): + name = actuator.get('name') + joint = actuator.get('joint') + if name and joint: + joint_actuators[joint] = name + + return joint_actuators + +def get_robot_mjcf_path(company_name: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: + """ + Get the path to the MJCF file of a robot. + :param company_name: The name of the company that created the robot. + :param robot_name: The name of the robot. + :param xml_name: The name of the XML file of the robot. + :return: The path to the MJCF file of the robot. + """ + xml_name = xml_name if xml_name is not None else robot_name + multiverse_resources = find_multiverse_resources_path() + if multiverse_resources is not None: + return os.path.join(multiverse_resources, 'robots', company_name, robot_name, 'mjcf', f'{robot_name}.xml') + return None + + +def get_resource_paths(dirname: str) -> List[str]: + resources_paths = ["../robots", "../worlds", "../objects"] + resources_paths = [ + os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( + resources_path) else resources_path + for resources_path in resources_paths + ] + + def add_directories(path: str) -> None: + with os.scandir(path) as entries: + for entry in entries: + if entry.is_dir(): + resources_paths.append(entry.path) + add_directories(entry.path) + + resources_path_copy = resources_paths.copy() + for resources_path in resources_path_copy: + add_directories(resources_path) + + return resources_paths + + +def find_multiverse_resources_path() -> Optional[str]: + """ + Find the path to the Multiverse resources directory. + """ + # Get the path to the Multiverse installation + multiverse_path = find_multiverse_path() + + # Check if the path to the Multiverse installation was found + if multiverse_path: + # Construct the path to the resources directory + resources_path = os.path.join(multiverse_path, 'resources') + + # Check if the resources directory exists + if os.path.exists(resources_path): + return resources_path + + return None + + +def find_multiverse_path() -> Optional[str]: + """ + Find the path to the Multiverse installation. + """ + # Get the value of PYTHONPATH environment variable + pythonpath = os.getenv('PYTHONPATH') + + # Check if PYTHONPATH is set + if pythonpath: + # Split the PYTHONPATH into individual paths using the platform-specific path separator + paths = pythonpath.split(os.pathsep) + + # Iterate through each path and check if 'Multiverse' is in it + for path in paths: + if 'multiverse' in path: + multiverse_path = path.split('multiverse')[0] + return multiverse_path + 'multiverse' + + return None diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py index b7a459ff9..4a6d16c12 100644 --- a/test/test_error_checkers.py +++ b/test/test_error_checkers.py @@ -4,7 +4,7 @@ from tf.transformations import quaternion_from_euler from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ +from pycram.worlds.multiverse_extras.error_checkers import calculate_angle_between_quaternions, \ PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 6ebfe04ff..4807e3af0 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -6,10 +6,10 @@ from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription -from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ +from pycram.worlds.multiverse_extras.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ +from pycram.worlds.multiverse_extras.goal_validator import GoalValidator, PoseGoalValidator, \ PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 91bdc52d2..4e7824506 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -4,18 +4,18 @@ import numpy as np import psutil -from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_multiply, quaternion_inverse, \ - quaternion_conjugate +from tf.transformations import quaternion_from_euler, quaternion_multiply from typing_extensions import Optional, List from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint -from pycram.datastructures.enums import ObjectType, Arms +from pycram.datastructures.enums import ObjectType, Arms, JointType from pycram.datastructures.pose import Pose from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription +from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ - calculate_quaternion_difference +from pycram.worlds.multiverse_extras.error_checkers import calculate_angle_between_quaternions +from pycram.worlds.multiverse_extras.helpers import parse_mjcf_actuators, get_robot_mjcf_path multiverse_installed = True try: @@ -56,6 +56,22 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.reset_world_and_remove_objects() + def test_parse_mjcf_actuators(self): + mjcf_file = get_robot_mjcf_path("pal_robotics", "tiago_dual") + self.assertTrue(os.path.exists(mjcf_file)) + joint_actuators = parse_mjcf_actuators(mjcf_file) + self.assertIsInstance(joint_actuators, dict) + self.assertTrue(len(joint_actuators) > 0) + self.assertTrue("arm_left_1_joint" in joint_actuators) + self.assertTrue("arm_right_1_joint" in joint_actuators) + self.assertTrue(joint_actuators["arm_right_1_joint"] == "arm_right_1_actuator") + + def test_get_actuator_for_joint(self): + robot = self.spawn_robot() + joint_name = "arm_right_1_joint" + actuator_name = robot.get_actuator_for_joint(robot.joints[joint_name]) + self.assertEqual(actuator_name, "arm_right_1_actuator") + def test_demo(self): extension = ObjectDescription.get_file_extension() @@ -81,20 +97,27 @@ def test_reset_world(self): milk = self.spawn_milk(set_position) milk.set_position(set_position) milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], set_position[:2]) + self.assert_list_is_equal(milk_position[:2], set_position[:2], delta=self.multiverse.acceptable_position_error) self.multiverse.reset_world() milk_pose = milk.get_pose() self.assert_list_is_equal(milk_pose.position_as_list()[:2], - milk.original_pose.position_as_list()[:2]) - self.assert_list_is_equal(milk_pose.orientation_as_list(), - milk.original_pose.orientation_as_list()) + milk.original_pose.position_as_list()[:2], + delta=self.multiverse.acceptable_position_error) + self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) + + def test_spawn_robot_with_actuators_directly_from_multiverse(self): + robot_name = "tiago_dual" + rdm = RobotDescriptionManager() + rdm.load_description(robot_name) + self.multiverse.spawn_robot(robot_name, Pose([-2, -2, 0.001])) def test_spawn_object(self): milk = self.spawn_milk([1, 1, 0.1]) self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() - self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 1]) - self.assert_list_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) + self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 1], + delta=self.multiverse.acceptable_position_error) + self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_remove_object(self): milk = self.spawn_milk([1, 1, 0.1]) @@ -112,13 +135,14 @@ def test_set_position(self): original_milk_position[0] += 1 milk.set_position(original_milk_position) milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], original_milk_position[:2]) + self.assert_list_is_equal(milk_position[:2], original_milk_position[:2], + delta=self.multiverse.acceptable_position_error) def test_update_position(self): milk = self.spawn_milk([1, 1, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], [1, 1]) + self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.acceptable_position_error) def test_set_joint_position(self): if self.multiverse.robot is None: @@ -127,10 +151,13 @@ def test_set_joint_position(self): robot = self.multiverse.robot step = 0.2 for joint in ['torso_lift_joint']: + joint_type = robot.joints[joint].type original_joint_position = robot.get_joint_position(joint) robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.01) + delta = self.multiverse.acceptable_position_error if joint_type == JointType.PRISMATIC \ + else self.multiverse.acceptable_orientation_error + self.assertAlmostEqual(joint_position, original_joint_position + step, delta=delta) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -162,7 +189,8 @@ def test_set_robot_position(self): new_position = [-3, -3, 0.001] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() - self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) + self.assert_list_is_equal(robot_position[:2], new_position[:2], + delta=self.multiverse.acceptable_position_error) self.tearDown() def test_set_robot_orientation(self): @@ -190,7 +218,8 @@ def test_attach_object(self): estimated_cup_position[0] += 1 milk.set_position(milk_position) new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2]) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], + self.multiverse.acceptable_position_error) def test_detach_object(self): for i in range(2): @@ -207,8 +236,10 @@ def test_detach_object(self): milk.set_position(milk_position) new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.005) - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], + self.multiverse.acceptable_position_error) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], + self.multiverse.acceptable_position_error) self.tearDown() def test_attach_with_robot(self): @@ -302,16 +333,26 @@ def spawn_cup(position: List) -> Object: return cup def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, - position_delta: float = 0.002, orientation_delta: float = 0.002): + position_delta: Optional[float] = None, orientation_delta: Optional[float] = None): + if position_delta is None: + position_delta = self.multiverse.acceptable_position_error + if orientation_delta is None: + orientation_delta = self.multiverse.acceptable_orientation_error self.assert_positon_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) - self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) + self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), + delta=orientation_delta) - def assert_positon_is_equal(self, position1: List[float], position2: List[float], delta: float = 0.02): + def assert_positon_is_equal(self, position1: List[float], position2: List[float], delta: Optional[float] = None): + if delta is None: + delta = self.multiverse.acceptable_position_error self.assert_list_is_equal(position1, position2, delta=delta) - def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], delta: float = 0.01): - self.assert_list_is_equal(orientation1, orientation2, delta=delta) + def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], + delta: Optional[float] = None): + if delta is None: + delta = self.multiverse.acceptable_orientation_error + self.assertAlmostEqual(calculate_angle_between_quaternions(orientation1, orientation2), 0, delta=delta) - def assert_list_is_equal(self, list1: List, list2: List, delta: float = 0.001): + def assert_list_is_equal(self, list1: List, list2: List, delta: float): for i in range(len(list1)): self.assertAlmostEqual(list1[i], list2[i], delta=delta) diff --git a/test/test_robot_description.py b/test/test_robot_description.py index 3e3985054..efedd43a6 100644 --- a/test/test_robot_description.py +++ b/test/test_robot_description.py @@ -3,7 +3,7 @@ from pycram.robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ CameraDescription, RobotDescriptionManager from pycram.datastructures.enums import Arms, GripperState -from urdf_parser_py.urdf import URDF +from pycram.object_descriptors.urdf import ObjectDescription as URDFObject class TestRobotDescription(unittest.TestCase): @@ -11,7 +11,7 @@ class TestRobotDescription(unittest.TestCase): @classmethod def setUpClass(cls): cls.path = str(pathlib.Path(__file__).parent.resolve()) + '/../resources/robots/' + "pr2" + '.urdf' - cls.urdf_obj = URDF.from_xml_file(cls.path) + cls.urdf_obj = URDFObject(cls.path) def test_robot_description_construct(self): robot_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", self.path) @@ -19,7 +19,7 @@ def test_robot_description_construct(self): self.assertEqual(robot_description.base_link, "base_link") self.assertEqual(robot_description.torso_link, "torso_lift_link") self.assertEqual(robot_description.torso_joint, "torso_lift_joint") - self.assertTrue(type(robot_description.urdf_object) is URDF) + self.assertTrue(type(robot_description.urdf_object) is URDFObject) self.assertEqual(len(robot_description.links), 88) self.assertEqual(len(robot_description.joints), 87) From 6f6b84913af3464ca38e51ec9b3886c80177acef Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 28 Jul 2024 00:09:28 +0200 Subject: [PATCH 082/189] [Multiverse] joint controller is added as an option instead of instant setting of joint value. --- src/pycram/datastructures/world.py | 2 +- src/pycram/worlds/multiverse.py | 74 +++++++++++-------- .../multiverse_communication/clients.py | 2 +- test/test_multiverse.py | 5 +- 4 files changed, 49 insertions(+), 34 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 935f4284d..b1a8b3258 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -92,7 +92,7 @@ class World(StateEntity, ABC): acceptable_position_error: float = 5e-2 # 5 cm acceptable_orientation_error: float = 10 * np.pi / 180 # 10 degrees acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) - use_percentage_of_goal: bool = True + use_percentage_of_goal: bool = False acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None # 50% """ The acceptable error for the position and orientation of an object/link. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 2f025ab53..e5649661e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -65,20 +65,16 @@ class Multiverse(World): is completed. """ - position_tol: float = 2e-3 - """ - The tolerance for position comparison. (e.g. for checking if the object has reached the desired position) - """ - - orientation_tol: float = 2e-3 - """ - The tolerance for orientation comparison. (e.g. for checking if the object has reached the desired orientation) + use_controller: Optional[bool] = None + """ + Whether to use the joint controller or not. """ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, - simulation: Optional[str] = None): + simulation: Optional[str] = None, + use_controller: Optional[bool] = None): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -86,6 +82,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. param simulation: The name of the simulation. + param use_controller: Whether to use the controller or not. """ self._make_sure_multiverse_resources_are_added() @@ -98,6 +95,9 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + if Multiverse.use_controller is None: + Multiverse.use_controller = use_controller + World.__init__(self, mode, is_prospection, simulation_frequency) self._init_clients() @@ -117,8 +117,10 @@ def _init_clients(self): self.api_requester: MultiverseAPI = client_manager.create_api_requester( self.simulation, is_prospection_world=self.is_prospection_world) - # self.joint_controller: MultiverseController = client_manager.create_controller( - # is_prospection_world=self.is_prospection_world) + self.joint_controller: Optional[MultiverseController] = None + if Multiverse.use_controller: + self.joint_controller: MultiverseController = client_manager.create_controller( + is_prospection_world=self.is_prospection_world) def _set_world_job_flags(self): self.let_pycram_move_attached_objects = False @@ -153,21 +155,21 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def spawn_robot(self, name: str, pose: Pose) -> None: + def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: """ Spawn the robot in the simulator. param robot_description: The robot description. param pose: The pose of the robot. return: The object of the robot. """ - # actuator_joint_commands = { - # actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] - # for joint_name, actuator_name in self.robot_joint_actuators.items() - # } - # self.joint_controller.init_controller(actuator_joint_commands) + actuator_joint_commands = { + actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] + for joint_name, actuator_name in self.robot_joint_actuators.items() + } + self.joint_controller.init_controller(actuator_joint_commands) self.writer.spawn_robot_with_actuators(name, pose.position_as_list(), - self.xyzw_to_wxyz(pose.orientation_as_list())) - # actuator_joint_commands) + self.xyzw_to_wxyz(pose.orientation_as_list()), + actuator_joint_commands) def get_joint_position_name(self, joint_type: JointType) -> MultiverseJointProperty: return self._joint_type_to_position_name[joint_type] @@ -202,8 +204,8 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> int: param pose: The pose of the object. return: The object id. """ - if object_type == ObjectType.ROBOT: - self.spawn_robot(name, pose) + if object_type == ObjectType.ROBOT and Multiverse.use_controller: + self.spawn_robot_with_controller(name, pose) else: self._set_body_pose(name, pose) @@ -239,20 +241,32 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), - [joint_position]) - # self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), self.get_joint_cmd_name(joint.type), - # [joint_position]) + if Multiverse.use_controller: + return self._reset_joint_position_using_controller(joint, joint_position) + else: + self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), + [joint_position]) + return True + + def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: + self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), self.get_joint_cmd_name(joint.type), + [joint_position]) return True @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: - data = {joint.name: {self.get_joint_position_name(joint.type): [position]} + if self.use_controller: + return self._set_multiple_joint_positions_using_controller(joint_positions) + else: + data = {joint.name: {self.get_joint_position_name(joint.type): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(data) + return True + + def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: + data = {self.get_actuator_for_joint(joint): {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} - self.writer.send_multiple_body_data_to_server(data) - # data = {self.get_actuator_for_joint(joint): {self.get_joint_cmd_name(joint.type): [position]} - # for joint, position in joint_positions.items()} - # self.joint_controller.send_multiple_body_data_to_server(data) + self.joint_controller.send_multiple_body_data_to_server(data) return True def get_joint_position(self, joint: Joint) -> Optional[float]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 5c57af2f8..878046f7f 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -473,7 +473,7 @@ def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. """ self.send_data_to_server([self.sim_time] + [0.0]*len(actuator_joint_commands), - send_meta_data={key: value for key, value in actuator_joint_commands.items()}) + send_meta_data=actuator_joint_commands) class MultiverseAPI(MultiverseClient): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4e7824506..779c83584 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -47,7 +47,8 @@ def setUpClass(cls): if not multiverse_installed: return cls.multiverse = Multiverse(simulation="pycram_test", - is_prospection=False) + is_prospection=False, + use_controller=False) @classmethod def tearDownClass(cls): @@ -109,7 +110,7 @@ def test_spawn_robot_with_actuators_directly_from_multiverse(self): robot_name = "tiago_dual" rdm = RobotDescriptionManager() rdm.load_description(robot_name) - self.multiverse.spawn_robot(robot_name, Pose([-2, -2, 0.001])) + self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001])) def test_spawn_object(self): milk = self.spawn_milk([1, 1, 0.1]) From e4f8df7bad1f0a3394347fd2561a3abd8a412b6e Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 29 Jul 2024 00:20:48 +0200 Subject: [PATCH 083/189] [Multiverse] virtual joints are excluded from controlled joints. Tests are passing. --- src/pycram/datastructures/dataclasses.py | 7 +++ src/pycram/datastructures/world.py | 26 ++++++++- src/pycram/world_concepts/world_object.py | 21 +++++-- src/pycram/worlds/bullet_world.py | 4 ++ src/pycram/worlds/multiverse.py | 55 +++++++++++++------ .../worlds/multiverse_datastructures/enums.py | 4 ++ test/test_multiverse.py | 4 +- 7 files changed, 96 insertions(+), 25 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index f884f7127..61d77a966 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -458,6 +458,13 @@ class VirtualMoveBaseJoints: translation_y: Optional[str] = VirtualMoveBaseJointName.LINEAR_Y.value angular_z: Optional[str] = VirtualMoveBaseJointName.ANGULAR_Z.value + @property + def names(self) -> List[str]: + """ + Returns the names of the virtual move base joints. + """ + return [self.translation_x, self.translation_y, self.angular_z] + def get_types(self) -> Dict[str, JointType]: """ Returns the joint types of the virtual move base joints. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b1a8b3258..7ed05a4ce 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -580,6 +580,20 @@ def get_object_orientation(self, obj: Object) -> List[float]: """ pass + @property + def robot_virtual_joints(self) -> List[Joint]: + """ + Returns the virtual joints of the robot. + """ + return [self.robot.joints[name] for name in self.robot_virtual_joints_names] + + @property + def robot_virtual_joints_names(self) -> List[str]: + """ + Returns the virtual joints of the robot. + """ + return self.robot_description.virtual_move_base_joints.names + def set_mobile_robot_pose(self, pose: Pose) -> None: """ Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when @@ -587,7 +601,17 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_multiple_joint_positions(goal) + self.robot.set_move_base_joint_positions(goal) + + @abstractmethod + def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints of an articulated object without using the controller. + + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. + :return: True if the set was successful, False otherwise. + """ + pass def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 76f398239..4773262a6 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -14,12 +14,12 @@ from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World +from ..datastructures.world_entity import WorldEntity from ..description import ObjectDescription, LinkDescription, Joint from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment -from ..datastructures.world_entity import WorldEntity Link = ObjectDescription.Link @@ -62,7 +62,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if pose is None: pose = Pose() if name in [obj.name for obj in self.world.objects]: - msg = f"An object with the name {name} already exists in the world,"\ + msg = f"An object with the name {name} already exists in the world," \ f" is_prospection_world: {self.world.is_prospection_world}" rospy.logerr(msg) raise ValueError(msg) @@ -953,6 +953,12 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) + def set_move_base_joint_positions(self, joint_positions: Dict[str, float]) -> None: + joint_positions = {self.joints[joint_name]: joint_position + for joint_name, joint_position in joint_positions.items()} + if self.world.set_multiple_joint_positions_without_controller(joint_positions): + self._update_on_joint_position_change() + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting @@ -963,10 +969,13 @@ def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> Non joint_positions = {self.joints[joint_name]: joint_position for joint_name, joint_position in joint_positions.items()} if self.world.set_multiple_joint_positions(joint_positions): - self.update_pose() - self._update_all_links_poses() - self.update_link_transforms() - self._set_attached_objects_poses() + self._update_on_joint_position_change() + + def _update_on_joint_position_change(self): + self.update_pose() + self._update_all_links_poses() + self.update_link_transforms() + self._set_attached_objects_poses() def get_joint_position(self, joint_name: str) -> float: """ diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 171a34445..6a9658726 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -182,6 +182,10 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} + def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: + self.set_multiple_joint_positions(joint_positions) + return True + def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index e5649661e..a36a67571 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -117,10 +117,8 @@ def _init_clients(self): self.api_requester: MultiverseAPI = client_manager.create_api_requester( self.simulation, is_prospection_world=self.is_prospection_world) - self.joint_controller: Optional[MultiverseController] = None - if Multiverse.use_controller: - self.joint_controller: MultiverseController = client_manager.create_controller( - is_prospection_world=self.is_prospection_world) + self.joint_controller: MultiverseController = client_manager.create_controller( + is_prospection_world=self.is_prospection_world) def _set_world_job_flags(self): self.let_pycram_move_attached_objects = False @@ -163,7 +161,7 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: return: The object of the robot. """ actuator_joint_commands = { - actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] + actuator_name: [self.get_joint_cmd_name(joint_name, self.robot_description.joint_types[joint_name]).value] for joint_name, actuator_name in self.robot_joint_actuators.items() } self.joint_controller.init_controller(actuator_joint_commands) @@ -174,8 +172,11 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: def get_joint_position_name(self, joint_type: JointType) -> MultiverseJointProperty: return self._joint_type_to_position_name[joint_type] - def get_joint_cmd_name(self, joint_type: JointType) -> MultiverseJointProperty: - return self._joint_type_to_cmd_name.get(joint_type, self.get_joint_position_name(joint_type)) + def get_joint_cmd_name(self, joint_name: str, joint_type: JointType) -> MultiverseJointProperty: + if joint_name in self.robot_virtual_joints_names: + return self.get_joint_position_name(joint_type) + else: + return self._joint_type_to_cmd_name[joint_type] def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, @@ -249,24 +250,46 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: return True def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: - self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), self.get_joint_cmd_name(joint.type), + self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), + self.get_joint_cmd_name(joint.name, joint.type), [joint_position]) return True @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: - if self.use_controller: - return self._set_multiple_joint_positions_using_controller(joint_positions) + + virtual_joint_positions = self.get_virtual_joint_positions(joint_positions) + if len(virtual_joint_positions) > 0: + self.set_multiple_joint_positions_without_controller(virtual_joint_positions) + + physical_joint_positions = self.get_physical_joint_positions(joint_positions) + + if Multiverse.use_controller: + self._set_multiple_joint_positions_using_controller(physical_joint_positions) else: - data = {joint.name: {self.get_joint_position_name(joint.type): [position]} - for joint, position in joint_positions.items()} - self.writer.send_multiple_body_data_to_server(data) + self._set_multiple_joint_positions_using_controller(physical_joint_positions) + + return True + + def get_physical_joint_positions(self, joint_positions: Dict[Joint, float]) -> Dict[str, float]: + return {joint: joint_positions[joint] for joint in joint_positions.keys() if + joint.name not in self.robot_virtual_joints_names} + + def get_virtual_joint_positions(self, joint_positions: Dict[Joint, float]) -> Dict[str, float]: + return {joint: joint_positions[joint] for joint in self.robot_virtual_joints if joint in joint_positions} + + def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: + not_controlled_joints_data = {joint.name: {self.get_joint_position_name(joint.type): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(not_controlled_joints_data) return True def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: - data = {self.get_actuator_for_joint(joint): {self.get_joint_cmd_name(joint.type): [position]} - for joint, position in joint_positions.items()} - self.joint_controller.send_multiple_body_data_to_server(data) + controlled_joints_data = {self.get_actuator_for_joint(joint): + {self.get_joint_cmd_name(joint.name, joint.type): [position]} + for joint, position in joint_positions.items() + if joint.name not in self.robot_virtual_joints_names} + self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True def get_joint_position(self, joint: Joint) -> Optional[float]: diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index cb001a789..85f3ea362 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -36,3 +36,7 @@ class MultiverseJointProperty(MultiverseProperty): PRISMATIC_JOINT_POSITION = "joint_tvalue" REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" + + def is_cmd(self): + return self in [MultiverseJointProperty.REVOLUTE_JOINT_CMD, + MultiverseJointProperty.PRISMATIC_JOINT_CMD] diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 779c83584..1d18be0e8 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -48,7 +48,7 @@ def setUpClass(cls): return cls.multiverse = Multiverse(simulation="pycram_test", is_prospection=False, - use_controller=False) + use_controller=True) @classmethod def tearDownClass(cls): @@ -244,7 +244,7 @@ def test_detach_object(self): self.tearDown() def test_attach_with_robot(self): - milk = self.spawn_milk([1, 1, 0.1]) + milk = self.spawn_milk([-1, -1, 0.1]) robot = self.spawn_robot() ee_link = self.multiverse.get_arm_tool_frame_link(Arms.RIGHT) # Get position of milk relative to robot end effector From 8c64ef4d49580e62f9e77c7f03406776217df7a4 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 29 Jul 2024 15:04:44 +0200 Subject: [PATCH 084/189] [Multiverse] Some cleaning up. --- src/pycram/datastructures/world.py | 12 +++-- src/pycram/world_concepts/world_object.py | 6 --- src/pycram/worlds/bullet_world.py | 4 -- src/pycram/worlds/multiverse.py | 54 ++++++++++------------- 4 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 7ed05a4ce..aca68199d 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -170,7 +170,13 @@ def get_actuator_for_joint(self, joint: Joint) -> str: """ Get the actuator name for a given joint. """ - return self.robot_joint_actuators.get(joint.name, joint.name) + return self.robot_joint_actuators[joint.name] + + def joint_has_actuator(self, joint: Joint) -> bool: + """ + Returns whether the joint has an actuator. + """ + return joint.name in self.robot_joint_actuators @property def robot_joint_actuators(self) -> Dict[str, str]: @@ -601,10 +607,10 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_move_base_joint_positions(goal) + self.robot.set_multiple_joint_positions(goal) @abstractmethod - def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: + def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object without using the controller. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 4773262a6..a6238cf30 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -953,12 +953,6 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) - def set_move_base_joint_positions(self, joint_positions: Dict[str, float]) -> None: - joint_positions = {self.joints[joint_name]: joint_position - for joint_name, joint_position in joint_positions.items()} - if self.world.set_multiple_joint_positions_without_controller(joint_positions): - self._update_on_joint_position_change() - def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 6a9658726..171a34445 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -182,10 +182,6 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} - def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: - self.set_multiple_joint_positions(joint_positions) - return True - def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a36a67571..483814ab4 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -161,7 +161,7 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: return: The object of the robot. """ actuator_joint_commands = { - actuator_name: [self.get_joint_cmd_name(joint_name, self.robot_description.joint_types[joint_name]).value] + actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] for joint_name, actuator_name in self.robot_joint_actuators.items() } self.joint_controller.init_controller(actuator_joint_commands) @@ -169,15 +169,6 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: self.xyzw_to_wxyz(pose.orientation_as_list()), actuator_joint_commands) - def get_joint_position_name(self, joint_type: JointType) -> MultiverseJointProperty: - return self._joint_type_to_position_name[joint_type] - - def get_joint_cmd_name(self, joint_name: str, joint_type: JointType) -> MultiverseJointProperty: - if joint_name in self.robot_virtual_joints_names: - return self.get_joint_position_name(joint_type) - else: - return self._joint_type_to_cmd_name[joint_type] - def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, obj_type: Optional[ObjectType] = None) -> int: @@ -243,7 +234,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: if Multiverse.use_controller: - return self._reset_joint_position_using_controller(joint, joint_position) + self._reset_joint_position_using_controller(joint, joint_position) else: self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), [joint_position]) @@ -251,34 +242,30 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), - self.get_joint_cmd_name(joint.name, joint.type), + self.get_joint_cmd_name(joint.type), [joint_position]) return True @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: - virtual_joint_positions = self.get_virtual_joint_positions(joint_positions) - if len(virtual_joint_positions) > 0: - self.set_multiple_joint_positions_without_controller(virtual_joint_positions) - - physical_joint_positions = self.get_physical_joint_positions(joint_positions) - if Multiverse.use_controller: - self._set_multiple_joint_positions_using_controller(physical_joint_positions) - else: - self._set_multiple_joint_positions_using_controller(physical_joint_positions) + controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) + if len(controlled_joints) > 0: + controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints} + self._set_multiple_joint_positions_using_controller(controlled_joint_positions) + joint_positions = {joint: joint_positions[joint] for joint in joint_positions.keys() + if joint not in controlled_joints} + if len(joint_positions) > 0: + self._set_multiple_joint_positions_without_controller(joint_positions) return True - def get_physical_joint_positions(self, joint_positions: Dict[Joint, float]) -> Dict[str, float]: - return {joint: joint_positions[joint] for joint in joint_positions.keys() if - joint.name not in self.robot_virtual_joints_names} - - def get_virtual_joint_positions(self, joint_positions: Dict[Joint, float]) -> Dict[str, float]: - return {joint: joint_positions[joint] for joint in self.robot_virtual_joints if joint in joint_positions} + def get_controlled_joints(self, joints: Optional[List[Joint]] = None) -> List[Joint]: + joints = self.robot.joints if joints is None else joints + return [joint for joint in joints if self.joint_has_actuator(joint)] - def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: + def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: not_controlled_joints_data = {joint.name: {self.get_joint_position_name(joint.type): [position]} for joint, position in joint_positions.items()} self.writer.send_multiple_body_data_to_server(not_controlled_joints_data) @@ -286,9 +273,8 @@ def set_multiple_joint_positions_without_controller(self, joint_positions: Dict[ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.name, joint.type): [position]} - for joint, position in joint_positions.items() - if joint.name not in self.robot_virtual_joints_names} + {self.get_joint_cmd_name(joint.type): [position]} + for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -305,6 +291,12 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str if data is not None: return {name: list(value.values())[0][0] for name, value in data.items()} + def get_joint_position_name(self, joint_type: JointType) -> MultiverseJointProperty: + return self._joint_type_to_position_name[joint_type] + + def get_joint_cmd_name(self, joint_type: JointType) -> MultiverseJointProperty: + return self._joint_type_to_cmd_name[joint_type] + def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) From f9b1e62f93481c93a45bb570c9b0f13dc9615ebe Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 29 Jul 2024 15:16:50 +0200 Subject: [PATCH 085/189] [Multiverse] Some cleaning up. --- src/pycram/datastructures/world.py | 10 ---------- test/test_multiverse.py | 1 - 2 files changed, 11 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index aca68199d..bb42c2981 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -609,16 +609,6 @@ def set_mobile_robot_pose(self, pose: Pose) -> None: goal = self.get_move_base_joint_goal(pose) self.robot.set_multiple_joint_positions(goal) - @abstractmethod - def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: - """ - Set the positions of multiple joints of an articulated object without using the controller. - - :param joint_positions: A dictionary with joint objects as keys and joint positions as values. - :return: True if the set was successful, False otherwise. - """ - pass - def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 1d18be0e8..d6f80a9ce 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -205,7 +205,6 @@ def test_set_robot_orientation(self): robot_orientation = self.multiverse.robot.get_orientation_as_list() quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) - # self.tearDown() def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) From 6bea9373ea8ce80938a264bd90582679ac468fd1 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 29 Jul 2024 22:21:07 +0200 Subject: [PATCH 086/189] [Multiverse] Added pycram actions to the demo test of multiverse. corrected ray_test_batch. added copy_to_world method in world_object Need to wait for pause functionality for multiverse. --- src/pycram/datastructures/world.py | 9 ++-- src/pycram/world_concepts/world_object.py | 16 +++--- src/pycram/worlds/multiverse.py | 46 ++++++++++++----- .../multiverse_communication/clients.py | 5 +- test/test_multiverse.py | 51 ++++++++++++++----- 5 files changed, 85 insertions(+), 42 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index bb42c2981..b0da4ea9c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -366,19 +366,18 @@ def remove_object(self, obj: Object) -> None: :param obj: The object to be removed. """ + while self.object_lock.locked(): time.sleep(0.1) self.object_lock.acquire() - obj.detach_all() + obj.detach_all() if obj.name != "floor": self.objects.remove(obj) - - if obj.name != "floor": self.remove_object_from_simulator(obj) - if World.robot == obj: World.robot = None + self.object_lock.release() def add_fixed_constraint(self, parent_link: Link, child_link: Link, @@ -1466,7 +1465,7 @@ def add_object(self, obj: Object) -> None: :param obj: The object to be added. """ - self.object_to_prospection_object_map[obj] = copy(obj) + self.object_to_prospection_object_map[obj] = obj.copy_to_prospection() def remove_object(self, obj: Object) -> None: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a6238cf30..239fc6cba 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -702,7 +702,7 @@ def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ for obj, attachment in attachments.items(): if self.world.is_prospection_world and not obj.world.is_prospection_world: - # In case this object is in the prospection world and the other object is not, the attachment will no + # In case this object is in the prospection world and the other object is not, the attachment will not # be set. continue if obj in self.attachments: @@ -1188,19 +1188,17 @@ def copy_to_prospection(self) -> Object: :return: The copied object in the prospection world. """ - obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), - self.world.prospection_world, self.color) - obj.current_state = self.current_state - return obj + return self.copy_to_world(self.world.prospection_world) - def __copy__(self) -> Object: + def copy_to_world(self, world: World) -> Object: """ - Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. + Copies this object to the given world. - :return: A copy of this object. + :param world: The world to which the object should be copied. + :return: The copied object in the given world. """ obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), - self.world.prospection_world, self.color) + world, self.color) obj.current_state = self.current_state return obj diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 483814ab4..74a11647d 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,5 +1,6 @@ import logging import os +from time import sleep import numpy as np from tf.transformations import quaternion_matrix @@ -70,11 +71,16 @@ class Multiverse(World): Whether to use the joint controller or not. """ + REMOVE_ROBOT_WAIT_TIME: float = 0.5 + """ + The time to wait after removing the robot from the simulator. + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: Optional[float] = 60.0, simulation: Optional[str] = None, - use_controller: Optional[bool] = None): + use_controller: Optional[bool] = False): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -233,11 +239,12 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - if Multiverse.use_controller: + if Multiverse.use_controller and self.joint_has_actuator(joint): self._reset_joint_position_using_controller(joint, joint_position) else: - self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), - [joint_position]) + self._set_multiple_joint_positions_without_controller({joint: joint_position}) + # self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), + # [joint_position]) return True def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: @@ -266,14 +273,13 @@ def get_controlled_joints(self, joints: Optional[List[Joint]] = None) -> List[Jo return [joint for joint in joints if self.joint_has_actuator(joint)] def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> bool: - not_controlled_joints_data = {joint.name: {self.get_joint_position_name(joint.type): [position]} - for joint, position in joint_positions.items()} - self.writer.send_multiple_body_data_to_server(not_controlled_joints_data) - return True + joints_data = {joint.name: {self.get_joint_position_name(joint.type): [position]} + for joint, position in joint_positions.items()} + self.writer.send_multiple_body_data_to_server(joints_data) def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -416,6 +422,8 @@ def remove_object_by_id(self, obj_id: int) -> None: def remove_object_from_simulator(self, obj: Object) -> None: if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) + if obj.obj_type == ObjectType.ROBOT: + sleep(self.REMOVE_ROBOT_WAIT_TIME) def add_constraint(self, constraint: Constraint) -> int: @@ -495,7 +503,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> return obj1_contact_points.get_points_of_object(obj2) def ray_test(self, from_position: List[float], to_position: List[float]) -> Optional[int]: - ray_test_result = self.ray_test_batch([from_position], [to_position]) + ray_test_result = self.ray_test_batch([from_position], [to_position])[0] return ray_test_result[0] if ray_test_result[0] != -1 else None def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], @@ -506,11 +514,20 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L ray_results = self.api_requester.get_objects_intersected_with_rays(from_positions, to_positions) results = [] for ray_result in ray_results: + results.append([]) if ray_result.intersected(): - results.append(self.floor.id if ray_result.body_name == "world" else - self.object_name_to_id[ray_result.body_name]) + body_name = ray_result.body_name + if body_name == "world": + results[-1].append(self.floor.id) + elif body_name in self.object_name_to_id.keys(): + results[-1].append(self.object_name_to_id[body_name]) + else: + for obj in self.objects: + if body_name in obj.links.keys(): + results[-1].append(obj.id) + break else: - results.append(-1) + results[-1].append(-1) return results def step(self): @@ -554,3 +571,6 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) + + def add_vis_axis(self, pose: Pose) -> None: + logging.warning("add_vis_axis is not implemented in Multiverse") diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 878046f7f..55680b31e 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -291,7 +291,8 @@ def join(self): class MultiverseWriter(MultiverseClient): - def __init__(self, name: str, port: int, simulation: Optional[str] = None, is_prospection_world: Optional[bool] = False, + def __init__(self, name: str, port: int, simulation: Optional[str] = None, + is_prospection_world: Optional[bool] = False, simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. @@ -472,7 +473,7 @@ def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None Initialize the controller by sending the controller data to the multiverse server. :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. """ - self.send_data_to_server([self.sim_time] + [0.0]*len(actuator_joint_commands), + self.send_data_to_server([self.sim_time] + [0.0] * len(actuator_joint_commands), send_meta_data=actuator_joint_commands) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index d6f80a9ce..5a98066e2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import unittest +from time import sleep import numpy as np import psutil @@ -8,14 +9,17 @@ from typing_extensions import Optional, List from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint -from pycram.datastructures.enums import ObjectType, Arms, JointType +from pycram.datastructures.enums import ObjectType, Arms, JointType, Grasp from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, NavigateAction, LookAtAction, \ + DetectAction, TransportAction, PickUpAction from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object from pycram.worlds.multiverse_extras.error_checkers import calculate_angle_between_quaternions from pycram.worlds.multiverse_extras.helpers import parse_mjcf_actuators, get_robot_mjcf_path +from pycram.process_module import simulated_robot, with_simulated_robot multiverse_installed = True try: @@ -76,22 +80,43 @@ def test_get_actuator_for_joint(self): def test_demo(self): extension = ObjectDescription.get_file_extension() - robot = self.spawn_robot(robot_name='tiago_dual', position=[1, 2, 0.01]) - apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + robot = self.spawn_robot(robot_name='tiago_dual', position=[1.3, 2, 0.01]) + # apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) - bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), - color=Color(1, 1, 0, 1)) - spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.6, 2.6, 1]), - color=Color(0, 0, 1, 1)) - bowl.attach(spoon) - bowl.set_position([2.5, 2.4, 1.02]) + # bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), + # color=Color(1, 1, 0, 1)) + # spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.6, 2.6, 1]), + # color=Color(0, 0, 1, 1)) + # bowl.attach(spoon) + # bowl.set_position([2.5, 2.4, 1.02]) pick_pose = Pose([2.7, 2.15, 1]) robot_desig = BelieveObject(names=["tiago_dual"]) - apartment_desig = BelieveObject(names=["apartment"]) + # apartment_desig = BelieveObject(names=["apartment"]) + + with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) + + # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + + @staticmethod + @with_simulated_robot + def move_and_detect(obj_type: ObjectType, pick_pose: Pose): + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[pick_pose]).resolve().perform() + + # object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + object_desig = BelieveObject(types=[obj_type]).resolve() + + return object_desig def test_reset_world(self): set_position = [1, 1, 0.1] @@ -294,8 +319,8 @@ def test_get_rays(self): milk = self.spawn_milk([1, 1, 0.1]) intersected_objects = self.multiverse.ray_test_batch([[1, 2, 0.1], [1, 2, 0.1]], [[1, 1.5, 0.1], [1, 1, 0.1]]) - self.assertTrue(intersected_objects[0] == -1) - self.assertTrue(intersected_objects[1] == milk.id) + self.assertTrue(intersected_objects[0][0] == -1) + self.assertTrue(intersected_objects[1][0] == milk.id) @staticmethod def spawn_big_bowl() -> Object: @@ -355,4 +380,4 @@ def assert_orientation_is_equal(self, orientation1: List[float], orientation2: L def assert_list_is_equal(self, list1: List, list2: List, delta: float): for i in range(len(list1)): - self.assertAlmostEqual(list1[i], list2[i], delta=delta) + self.assertAlmostEqual(list1[i], list2[i], delta=delta) \ No newline at end of file From 5ec015d9ddd7bcaa5c5ac60c0ec05d2bc8f8fe52 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 29 Jul 2024 23:41:07 +0200 Subject: [PATCH 087/189] [Multiverse] Added pause and resume simulation. Implemented the step function. tests are running. --- src/pycram/worlds/multiverse.py | 9 +++++++-- .../worlds/multiverse_communication/clients.py | 14 +++++++++++++- .../worlds/multiverse_datastructures/enums.py | 2 ++ test/test_multiverse.py | 14 +++++++------- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 74a11647d..7635f2d4e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -115,6 +115,8 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if not self.is_prospection_world: self._spawn_floor() + self.api_requester.pause_simulation() + def _init_clients(self): client_manager = MultiverseClientManager(self.simulation_wait_time_factor) self.reader: MultiverseReader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) @@ -451,12 +453,13 @@ def remove_constraint(self, constraint_id) -> None: self.api_requester.detach(constraint) def perform_collision_detection(self) -> None: - logging.warning("perform_collision_detection is not implemented in Multiverse") + self.step() def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ + self.step() multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -531,7 +534,9 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L return results def step(self): - logging.warning("step is not implemented in Multiverse") + self.api_requester.unpause_simulation() + sleep(30 / self.simulation_frequency) + self.api_requester.pause_simulation() def save_physics_simulator_state(self) -> int: logging.warning("save_physics_simulator_state is not implemented in Multiverse") diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 55680b31e..1c1984d67 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -688,13 +688,25 @@ def _get_contact_points(self, object_name) -> Dict[API, List]: API.GET_CONSTRAINT_EFFORT: [object_name] }) + def pause_simulation(self) -> None: + """ + Pause the simulation. + """ + self._request_single_api_callback(API.PAUSE) + + def unpause_simulation(self) -> None: + """ + Unpause the simulation. + """ + self._request_single_api_callback(API.UNPAUSE) + def _request_single_api_callback(self, api_name: API, *params) -> List[str]: """ Request a single API callback from the server. param api_data: The API data to request the callback. return: The API response as a list of strings. """ - response = self._request_apis_callbacks({api_name: list(params)}) + response = self._request_apis_callbacks({api_name: params}) return response[api_name] def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[str]]: diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index 85f3ea362..8249f0a4f 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -12,6 +12,8 @@ class MultiverseAPIName(Enum): DETACH = "detach" GET_RAYS = "get_rays" EXIST = "exist" + PAUSE = "pause" + UNPAUSE = "unpause" class MultiverseProperty(Enum): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5a98066e2..5df887594 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -52,7 +52,7 @@ def setUpClass(cls): return cls.multiverse = Multiverse(simulation="pycram_test", is_prospection=False, - use_controller=True) + use_controller=False) @classmethod def tearDownClass(cls): @@ -81,9 +81,9 @@ def test_demo(self): extension = ObjectDescription.get_file_extension() robot = self.spawn_robot(robot_name='tiago_dual', position=[1.3, 2, 0.01]) - # apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") - milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), + milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) # bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), # color=Color(1, 1, 0, 1)) @@ -92,15 +92,15 @@ def test_demo(self): # bowl.attach(spoon) # bowl.set_position([2.5, 2.4, 1.02]) - pick_pose = Pose([2.7, 2.15, 1]) + pick_pose = Pose([2.6, 2.15, 1]) robot_desig = BelieveObject(names=["tiago_dual"]) - # apartment_desig = BelieveObject(names=["apartment"]) + apartment_desig = BelieveObject(names=["apartment"]) with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.25]).resolve().perform() + MoveTorsoAction([0.3]).resolve().perform() milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) @@ -298,7 +298,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.1], [0, -0.707, 0, 0.707]) + milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) cup = self.spawn_cup([1, 1, 0.1]) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) From 1c7f1f025352f16bc2f3d0cd3b90d1cdc6ff3a86 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 30 Jul 2024 00:23:07 +0200 Subject: [PATCH 088/189] [Multiverse] fixing prospection robot not having virtual joints. --- src/pycram/datastructures/world.py | 7 ++++--- src/pycram/worlds/multiverse.py | 2 +- test/test_multiverse.py | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b0da4ea9c..71c9aef86 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -599,14 +599,15 @@ def robot_virtual_joints_names(self) -> List[str]: """ return self.robot_description.virtual_move_base_joints.names - def set_mobile_robot_pose(self, pose: Pose) -> None: + def set_mobile_robot_pose(self, robot_obj: Object, pose: Pose) -> None: """ Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). - param pose: The target pose. + :param robot_obj: The robot object. + :param pose: The target pose. """ goal = self.get_move_base_joint_goal(pose) - self.robot.set_multiple_joint_positions(goal) + robot_obj.set_multiple_joint_positions(goal) def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7635f2d4e..b52b0f451 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -326,7 +326,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): - self.set_mobile_robot_pose(pose) + self.set_mobile_robot_pose(obj ,pose) else: self._set_body_pose(obj.name, pose) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5df887594..dd9b2d252 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -104,7 +104,7 @@ def test_demo(self): milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) - # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() @staticmethod @with_simulated_robot From 1ce3c7bdf9a01c8fb92257b43c81c487c2f21018 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 30 Jul 2024 01:09:55 +0200 Subject: [PATCH 089/189] [Multiverse] fixed prospection robot problem. --- src/pycram/world_concepts/world_object.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 239fc6cba..b5af1eb4a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -84,7 +84,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.description.update_description_from_file(self.path) - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: + if self.obj_type == ObjectType.ROBOT: self._add_virtual_move_base_joints() self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name From 036b13ef8177ff976b0dbbad70ce76cf5f7d2ad2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 30 Jul 2024 18:01:50 +0200 Subject: [PATCH 090/189] [AbstractWorld] Added a world_conf.py in a new config folder to put constant configurations. Added object files in the resources to a new objects folder. --- .../pycram_multiverse_demo/multiverse_demo.py | 90 ------------- resources/{ => objects}/IAI_kitchen.urdf | 0 resources/{ => objects}/Static_CokeBottle.stl | Bin .../{ => objects}/Static_MilkPitcher.stl | Bin resources/{ => objects}/apartment.urdf | 0 resources/{ => objects}/apartment_bowl.stl | Bin .../apartment_without_walls.urdf | 0 resources/{ => objects}/bowl.stl | Bin resources/{ => objects}/box.urdf | 0 resources/{ => objects}/breakfast_cereal.stl | Bin resources/{ => objects}/broken_kitchen.urdf | 0 resources/{ => objects}/jeroen_cup.stl | Bin resources/{ => objects}/kitchen.urdf | 0 resources/{ => objects}/milk.stl | Bin resources/{ => objects}/plane.obj | 0 resources/{ => objects}/plane.urdf | 0 resources/{ => objects}/spoon.stl | Bin src/pycram/cache_manager.py | 27 ++-- src/pycram/config/__init__.py | 0 src/pycram/config/world_conf.py | 34 +++++ src/pycram/datastructures/dataclasses.py | 10 +- src/pycram/datastructures/world.py | 7 +- src/pycram/description.py | 123 +++++++++++------- src/pycram/object_descriptors/urdf.py | 63 ++++++--- test/test_cache_manager.py | 7 +- 25 files changed, 184 insertions(+), 177 deletions(-) delete mode 100644 demos/pycram_multiverse_demo/multiverse_demo.py rename resources/{ => objects}/IAI_kitchen.urdf (100%) rename resources/{ => objects}/Static_CokeBottle.stl (100%) rename resources/{ => objects}/Static_MilkPitcher.stl (100%) rename resources/{ => objects}/apartment.urdf (100%) rename resources/{ => objects}/apartment_bowl.stl (100%) rename resources/{ => objects}/apartment_without_walls.urdf (100%) rename resources/{ => objects}/bowl.stl (100%) rename resources/{ => objects}/box.urdf (100%) rename resources/{ => objects}/breakfast_cereal.stl (100%) rename resources/{ => objects}/broken_kitchen.urdf (100%) rename resources/{ => objects}/jeroen_cup.stl (100%) rename resources/{ => objects}/kitchen.urdf (100%) rename resources/{ => objects}/milk.stl (100%) rename resources/{ => objects}/plane.obj (100%) rename resources/{ => objects}/plane.urdf (100%) rename resources/{ => objects}/spoon.stl (100%) create mode 100644 src/pycram/config/__init__.py create mode 100644 src/pycram/config/world_conf.py diff --git a/demos/pycram_multiverse_demo/multiverse_demo.py b/demos/pycram_multiverse_demo/multiverse_demo.py deleted file mode 100644 index ab993e0d2..000000000 --- a/demos/pycram_multiverse_demo/multiverse_demo.py +++ /dev/null @@ -1,90 +0,0 @@ -from pycram.worlds.multiverse import Multiverse -from pycram.designators.action_designator import * -from pycram.designators.location_designator import * -from pycram.designators.object_designator import * -from pycram.datastructures.enums import ObjectType -from pycram.datastructures.pose import Pose -from pycram.process_module import simulated_robot, with_simulated_robot -from pycram.object_descriptors.urdf import ObjectDescription -from pycram.world_concepts.world_object import Object -from pycram.datastructures.dataclasses import Color - -extension = ObjectDescription.get_file_extension() - -world = Multiverse() -robot = Object("tiago_dual", ObjectType.ROBOT, f"tiago_dual{extension}", pose=Pose([1, 2, 0])) -apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") - -milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.5, 2, 1.02]), - color=Color(1, 0, 0, 1)) -spoon = Object("soup_spoon", ObjectType.SPOON, f"soup_spoon{extension}", pose=Pose([2.4, 2.2, 0.85]), - color=Color(0, 0, 1, 1)) -bowl = Object("big_bowl", ObjectType.BOWL, f"big_bowl{extension}", pose=Pose([2.5, 2.2, 1.02]), - color=Color(1, 1, 0, 1)) -apartment.attach(spoon, 'cabinet10_drawer_top') - -pick_pose = Pose([2.7, 2.15, 1]) - -robot_desig = BelieveObject(names=["tiago_dual"]) -apartment_desig = BelieveObject(names=["apartment"]) - - -@with_simulated_robot -def move_and_detect(obj_type): - NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() - - LookAtAction(targets=[pick_pose]).resolve().perform() - - object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() - - return object_desig - - -with simulated_robot: - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.25]).resolve().perform() - - milk_desig = move_and_detect(ObjectType.MILK) - - TransportAction(milk_desig, ["left"], [Pose([4.8, 3.55, 0.8])]).resolve().perform() - - bowl_desig = move_and_detect(ObjectType.BOWL) - - TransportAction(bowl_desig, ["left"], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() - - # Finding and navigating to the drawer holding the spoon - handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) - drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), - robot_desig=robot_desig.resolve()).resolve() - - NavigateAction([drawer_open_loc.pose]).resolve().perform() - - OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() - spoon.detach(apartment) - - # Detect and pickup the spoon - LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform() - - spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() - - pickup_arm = "left" if drawer_open_loc.arms[0] == "right" else "right" - PickUpAction(spoon_desig, [pickup_arm], ["top"]).resolve().perform() - - ParkArmsAction([Arms.LEFT if pickup_arm == "left" else Arms.RIGHT]).resolve().perform() - - CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() - - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.15]).resolve().perform() - - # Find a pose to place the spoon, move and then place it - spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1]) - placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() - - NavigateAction([placing_loc.pose]).resolve().perform() - - PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() - - ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/resources/IAI_kitchen.urdf b/resources/objects/IAI_kitchen.urdf similarity index 100% rename from resources/IAI_kitchen.urdf rename to resources/objects/IAI_kitchen.urdf diff --git a/resources/Static_CokeBottle.stl b/resources/objects/Static_CokeBottle.stl similarity index 100% rename from resources/Static_CokeBottle.stl rename to resources/objects/Static_CokeBottle.stl diff --git a/resources/Static_MilkPitcher.stl b/resources/objects/Static_MilkPitcher.stl similarity index 100% rename from resources/Static_MilkPitcher.stl rename to resources/objects/Static_MilkPitcher.stl diff --git a/resources/apartment.urdf b/resources/objects/apartment.urdf similarity index 100% rename from resources/apartment.urdf rename to resources/objects/apartment.urdf diff --git a/resources/apartment_bowl.stl b/resources/objects/apartment_bowl.stl similarity index 100% rename from resources/apartment_bowl.stl rename to resources/objects/apartment_bowl.stl diff --git a/resources/apartment_without_walls.urdf b/resources/objects/apartment_without_walls.urdf similarity index 100% rename from resources/apartment_without_walls.urdf rename to resources/objects/apartment_without_walls.urdf diff --git a/resources/bowl.stl b/resources/objects/bowl.stl similarity index 100% rename from resources/bowl.stl rename to resources/objects/bowl.stl diff --git a/resources/box.urdf b/resources/objects/box.urdf similarity index 100% rename from resources/box.urdf rename to resources/objects/box.urdf diff --git a/resources/breakfast_cereal.stl b/resources/objects/breakfast_cereal.stl similarity index 100% rename from resources/breakfast_cereal.stl rename to resources/objects/breakfast_cereal.stl diff --git a/resources/broken_kitchen.urdf b/resources/objects/broken_kitchen.urdf similarity index 100% rename from resources/broken_kitchen.urdf rename to resources/objects/broken_kitchen.urdf diff --git a/resources/jeroen_cup.stl b/resources/objects/jeroen_cup.stl similarity index 100% rename from resources/jeroen_cup.stl rename to resources/objects/jeroen_cup.stl diff --git a/resources/kitchen.urdf b/resources/objects/kitchen.urdf similarity index 100% rename from resources/kitchen.urdf rename to resources/objects/kitchen.urdf diff --git a/resources/milk.stl b/resources/objects/milk.stl similarity index 100% rename from resources/milk.stl rename to resources/objects/milk.stl diff --git a/resources/plane.obj b/resources/objects/plane.obj similarity index 100% rename from resources/plane.obj rename to resources/objects/plane.obj diff --git a/resources/plane.urdf b/resources/objects/plane.urdf similarity index 100% rename from resources/plane.urdf rename to resources/objects/plane.urdf diff --git a/resources/spoon.stl b/resources/objects/spoon.stl similarity index 100% rename from resources/spoon.stl rename to resources/objects/spoon.stl diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 7c3e73913..d99f8094b 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -22,24 +22,25 @@ class CacheManager: def __init__(self, cache_dir: str, data_directory: List[str]): """ - Initializes the CacheManager. + Initialize the CacheManager. :param cache_dir: The directory where the cached files are stored. :param data_directory: The directory where all resource files are stored. """ self.cache_dir = cache_dir self.data_directory = data_directory + self.clear_cache() def clear_cache(self): """ - Clears the cache directory. + Clear the cache directory. """ self.delete_cache_dir() self.create_cache_dir_if_not_exists() def delete_cache_dir(self): """ - Deletes the cache directory. + Delete the cache directory. """ if pathlib.Path(self.cache_dir).exists(): shutil.rmtree(self.cache_dir) @@ -47,7 +48,7 @@ def delete_cache_dir(self): def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, object_description: 'ObjectDescription', object_name: str) -> str: """ - Checks if the file is already in the cache directory, if not it will be preprocessed and saved in the cache. + Check if the file is already in the cache directory, if not preprocess and save in the cache. :param path: The path of the file to preprocess and save in the cache directory. :param ignore_cached_files: If True, the file will be preprocessed and saved in the cache directory even if it @@ -73,7 +74,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, def generate_description_and_write_to_cache(self, path: str, name: str, extension: str, cache_path: str, object_description: 'ObjectDescription') -> None: """ - Generates the description from the file at the given path and writes it to the cache directory. + Generate the description from the file at the given path and write it to the cache directory. :param path: The path of the file to preprocess. :param name: The name of the object. @@ -87,7 +88,7 @@ def generate_description_and_write_to_cache(self, path: str, name: str, extensio @staticmethod def write_to_cache(description_string: str, cache_path: str) -> None: """ - Writes the description string to the cache directory. + Write the description string to the cache directory. :param description_string: The description string to write to the cache directory. :param cache_path: The path of the file in the cache directory. @@ -97,8 +98,8 @@ def write_to_cache(description_string: str, cache_path: str) -> None: def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: """ - Looks for a file in the data directory of the World. If the file is not found in the data directory, this method - raises a FileNotFoundError. + Look for a file in the data directory of the World. If the file is not found in the data directory, raise a + FileNotFoundError. :param path_object: The pathlib object of the file to look for. """ @@ -116,14 +117,14 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: def create_cache_dir_if_not_exists(self): """ - Creates the cache directory if it does not exist. + Create the cache directory if it does not exist. """ if not pathlib.Path(self.cache_dir).exists(): os.mkdir(self.cache_dir) def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: """ - Checks if the file in the given path is already cached or if + Check if the file in the given path is already cached or if there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from the parameter server is used. @@ -135,7 +136,7 @@ def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: def check_with_extension(self, path: str) -> bool: """ - Checks if the file in the given ath exists in the cache directory including file extension. + Check if the file in the given ath exists in the cache directory including file extension. :param path: The path of the file to check. """ @@ -145,8 +146,8 @@ def check_with_extension(self, path: str) -> bool: def check_without_extension(self, path: str, object_description: 'ObjectDescription') -> bool: """ - Checks if the file in the given path exists in the cache directory without file extension, - the extension is added after the file name manually in this case. + Check if the file in the given path exists in the cache directory the given file extension. + Instead, replace the given extension with the extension of the used ObjectDescription and check for that one. :param path: The path of the file to check. :param object_description: The object description of the file. diff --git a/src/pycram/config/__init__.py b/src/pycram/config/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py new file mode 100644 index 000000000..a8f54647c --- /dev/null +++ b/src/pycram/config/world_conf.py @@ -0,0 +1,34 @@ +import os + +import numpy as np +from typing_extensions import Tuple + +resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') +""" +Global reference for the resources path, this is used to search for the description files of the robot and + the objects. +""" + +cache_dir: str = os.path.join(resources_path, 'cached') +""" +Global reference for the cache directory, this is used to cache the description files of the robot and the objects. +""" + +prospection_world_prefix: str = "prospection_" +""" +The prefix for the prospection world name. +""" + +acceptable_position_error: float = 5e-3 +acceptable_orientation_error: float = 10 * np.pi / 180 +acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) +use_percentage_of_goal: bool = True +acceptable_percentage_of_goal: float = 0.5 +""" +The acceptable error for the position and orientation of an object/link. +""" + +raise_goal_validator_error: bool = False +""" +Whether to raise an error if the goals are not achieved. +""" \ No newline at end of file diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index f884f7127..8c7e274ac 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -348,6 +348,9 @@ def __repr__(self): ClosestPoint = ContactPoint +""" +The closest point between two objects which has the same structure as ContactPoint. +""" class ContactPointsList(list): @@ -436,6 +439,9 @@ def __repr__(self): ClosestPointsList = ContactPointsList +""" +The list of closest points which has same structure as ContactPointsList. +""" @dataclass @@ -460,7 +466,7 @@ class VirtualMoveBaseJoints: def get_types(self) -> Dict[str, JointType]: """ - Returns the joint types of the virtual move base joints. + Return the joint types of the virtual move base joints. """ return {self.translation_x: JointType.PRISMATIC, self.translation_y: JointType.PRISMATIC, @@ -468,7 +474,7 @@ def get_types(self) -> Dict[str, JointType]: def get_axes(self) -> Dict[str, Point]: """ - Returns the axes (i.e. The axis on which the joint moves) of the virtual move base joints. + Return the axes (i.e. The axis on which the joint moves) of the virtual move base joints. """ return {self.translation_x: Point(1, 0, 0), self.translation_y: Point(0, 1, 0), diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 60ac5e45b..cbdbb1511 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -28,6 +28,7 @@ from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event +from ..config import world_conf as conf if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -131,7 +132,7 @@ class World(StateEntity, ABC): the URDF with the name of the URDF on the parameter server. """ - resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') + resources_path = conf.resources_path """ Global reference for the resources path, this is used to search for the description files of the robot and the objects. @@ -143,7 +144,7 @@ class World(StateEntity, ABC): the objects, and the cached files. """ - cache_dir: str = os.path.join(resources_path, 'cached') + cache_dir: str = conf.cache_dir """ Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ @@ -153,7 +154,7 @@ class World(StateEntity, ABC): Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ - prospection_world_prefix: str = "prospection_" + prospection_world_prefix: str = conf.prospection_world_prefix """ The prefix for the prospection world name. """ diff --git a/src/pycram/description.py b/src/pycram/description.py index c19c11b3f..d9ca6a829 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -20,7 +20,7 @@ class EntityDescription(ABC): """ - A class that represents a description of an entity. This can be a link, joint or object description. + A description of an entity. This can be a link, joint or object description. """ @property @@ -42,7 +42,7 @@ def name(self) -> str: class LinkDescription(EntityDescription): """ - A class that represents a link description of an object. + A link description of an object. """ def __init__(self, parsed_link_description: Any): @@ -52,7 +52,7 @@ def __init__(self, parsed_link_description: Any): @abstractmethod def geometry(self) -> Union[VisualShape, None]: """ - Returns the geometry type of the collision element of this link. + The geometry type of the collision element of this link. """ pass @@ -85,7 +85,7 @@ def axis(self) -> Point: @abstractmethod def has_limits(self) -> bool: """ - Checks if this joint has limits. + Check if this joint has limits. :return: True if the joint has limits, False otherwise. """ @@ -169,7 +169,7 @@ def pose(self) -> Pose: @property def transform(self) -> Transform: """ - Returns the transform of this entity. + The transform of this entity. :return: The transform of this entity. """ @@ -179,7 +179,7 @@ def transform(self) -> Transform: @abstractmethod def tf_frame(self) -> str: """ - Returns the tf frame of this entity. + The tf frame of this entity. :return: The tf frame of this entity. """ @@ -195,7 +195,7 @@ def object_id(self) -> int: class Link(ObjectEntity, LinkDescription, ABC): """ - Represents a link of an Object in the World. + A link of an Object in the World. """ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): @@ -205,6 +205,27 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self.constraint_ids: Dict[Link, int] = {} self._update_pose() + def set_pose(self, pose: Pose) -> None: + """ + Set the pose of this link to the given pose. + NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints + that can allow the link to be at the given pose. + :param pose: The target pose for this link. + """ + self.object.set_pose(self.get_object_pose_given_link_pose(pose)) + + def get_object_pose_given_link_pose(self, pose): + return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() + + def get_pose_given_object_pose(self, pose): + return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() + + def get_transform_from_root_link(self) -> Transform: + return self.get_transform_from_link(self.object.root_link) + + def get_transform_to_root_link(self) -> Transform: + return self.get_transform_to_link(self.object.root_link) + @property def current_state(self) -> LinkState: return LinkState(self.constraint_ids.copy()) @@ -215,7 +236,7 @@ def current_state(self, link_state: LinkState) -> None: def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional[Transform] = None) -> int: """ - Adds a fixed constraint between this link and the given link, used to create attachments for example. + Add a fixed constraint between this link and the given link, to create attachments for example. :param child_link: The child link to which a fixed constraint should be added. :param transform: The transformation between the two links. @@ -230,7 +251,7 @@ def add_fixed_constraint_with_link(self, child_link: 'Link', transform: Optional def remove_constraint_with_link(self, child_link: 'Link') -> None: """ - Removes the constraint between this link and the given link. + Remove the constraint between this link and the given link. :param child_link: The child link of the constraint that should be removed. """ @@ -242,7 +263,7 @@ def remove_constraint_with_link(self, child_link: 'Link') -> None: @property def is_only_link(self) -> bool: """ - Returns whether this link is the only link of the object. + Whether this link is the only link of the object. :return: True if this link is the only link, False otherwise. """ @@ -251,7 +272,7 @@ def is_only_link(self) -> bool: @property def is_root(self) -> bool: """ - Returns whether this link is the root link of the object. + Whether this link is the root link of the object. :return: True if this link is the root link, False otherwise. """ @@ -270,7 +291,7 @@ def set_pose(self, pose: Pose) -> None: def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: """ - Updates the transformation of this link at the given time. + Update the transformation of this link at the given time. :param transform_time: The time at which the transformation should be updated. """ @@ -278,7 +299,7 @@ def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: def get_transform_to_link(self, link: 'Link') -> Transform: """ - Returns the transformation from this link to the given link. + Return the transformation from this link to the given link. :param link: The link to which the transformation should be returned. :return: A Transform object with the transformation from this link to the given link. @@ -287,7 +308,7 @@ def get_transform_to_link(self, link: 'Link') -> Transform: def get_transform_from_link(self, link: 'Link') -> Transform: """ - Returns the transformation from the given link to this link. + Return the transformation from the given link to this link. :param link: The link from which the transformation should be returned. :return: A Transform object with the transformation from the given link to this link. @@ -296,7 +317,7 @@ def get_transform_from_link(self, link: 'Link') -> Transform: def get_pose_wrt_link(self, link: 'Link') -> Pose: """ - Returns the pose of this link with respect to the given link. + Return the pose of this link with respect to the given link. :param link: The link with respect to which the pose should be returned. :return: A Pose object with the pose of this link with respect to the given link. @@ -305,7 +326,7 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of this link. + Return the axis aligned bounding box of this link. :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. """ @@ -314,7 +335,7 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: @property def position(self) -> Point: """ - The getter for the position of the link relative to the world frame. + The position of the link relative to the world frame. :return: A Point object containing the position of the link relative to the world frame. """ @@ -323,7 +344,7 @@ def position(self) -> Point: @property def position_as_list(self) -> List[float]: """ - The getter for the position of the link relative to the world frame as a list. + The position of the link relative to the world frame as a list. :return: A list containing the position of the link relative to the world frame. """ @@ -332,7 +353,7 @@ def position_as_list(self) -> List[float]: @property def orientation(self) -> Quaternion: """ - The getter for the orientation of the link relative to the world frame. + The orientation of the link relative to the world frame. :return: A Quaternion object containing the orientation of the link relative to the world frame. """ @@ -341,7 +362,7 @@ def orientation(self) -> Quaternion: @property def orientation_as_list(self) -> List[float]: """ - The getter for the orientation of the link relative to the world frame as a list. + The orientation of the link relative to the world frame as a list. :return: A list containing the orientation of the link relative to the world frame. """ @@ -349,7 +370,7 @@ def orientation_as_list(self) -> List[float]: def _update_pose(self) -> None: """ - Updates the current pose of this link from the world. + Update the current pose of this link from the world. """ self._current_pose = self.world.get_link_pose(self) @@ -382,7 +403,7 @@ def get_origin_transform(self) -> Transform: @property def color(self) -> Color: """ - The getter for the rgba_color of this link. + The rgba_color of this link. :return: A Color object containing the rgba_color of this link. """ @@ -391,7 +412,7 @@ def color(self) -> Color: @color.setter def color(self, color: Color) -> None: """ - The setter for the color of this link, could be rgb or rgba. + Set the color of this link, could be rgb or rgba. :param color: The color as a list of floats, either rgb or rgba. """ @@ -423,8 +444,8 @@ def __hash__(self): class RootLink(Link, ABC): """ - Represents the root link of an Object in the World. - It differs from the normal AbstractLink class in that the pose ande the tf_frame is the same as that of the object. + The root link of an Object in the World. + This differs from the normal AbstractLink class in that the pose and the tf_frame is the same as that of the object. """ def __init__(self, obj: Object): @@ -433,7 +454,7 @@ def __init__(self, obj: Object): @property def tf_frame(self) -> str: """ - Returns the tf frame of the root link, which is the same as the tf frame of the object. + Return the tf frame of the root link, which is the same as the tf frame of the object. """ return self.object.tf_frame @@ -446,7 +467,7 @@ def __copy__(self): class Joint(ObjectEntity, JointDescription, ABC): """ - Represents a joint of an Object in the World. + Represent a joint of an Object in the World. """ def __init__(self, _id: int, @@ -466,7 +487,7 @@ def tf_frame(self) -> str: @property def pose(self) -> Pose: """ - Returns the pose of this joint. The pose is the pose of the child link of this joint. + Return the pose of this joint. The pose is the pose of the child link of this joint. :return: The pose of this joint. """ @@ -474,14 +495,14 @@ def pose(self) -> Pose: def _update_position(self) -> None: """ - Updates the current position of the joint from the physics simulator. + Update the current position of the joint from the physics simulator. """ self._current_position = self.world.get_joint_position(self) @property def parent_link(self) -> Link: """ - Returns the parent link of this joint. + Return the parent link of this joint. :return: The parent link as a AbstractLink object. """ @@ -490,7 +511,7 @@ def parent_link(self) -> Link: @property def child_link(self) -> Link: """ - Returns the child link of this joint. + Return the child link of this joint. :return: The child link as a AbstractLink object. """ @@ -508,7 +529,7 @@ def reset_position(self, position: float) -> None: def get_object_id(self) -> int: """ - Returns the id of the object to which this joint belongs. + Return the id of the object to which this joint belongs. :return: The integer id of the object to which this joint belongs. """ @@ -517,8 +538,8 @@ def get_object_id(self) -> int: @position.setter def position(self, joint_position: float) -> None: """ - Sets the position of the given joint to the given joint pose. If the pose is outside the joint limits, - an error will be printed. However, the joint will be set either way. + Set the position of the given joint to the given joint pose. If the pose is outside the joint limits, + issue a warning. However, set the joint either way. :param joint_position: The target pose for this joint """ @@ -553,7 +574,7 @@ def current_state(self) -> JointState: @current_state.setter def current_state(self, joint_state: JointState) -> None: """ - Updates the current state of this joint from the given joint state if the position is different. + Update the current state of this joint from the given joint state if the position is different. :param joint_state: The joint state to update from. """ @@ -598,12 +619,23 @@ def __init__(self, path: Optional[str] = None): else: self._parsed_description = None + self.virtual_joint_names: List[str] = [] + + def is_joint_virtual(self, name: str) -> bool: + """ + Return whether the joint with the given name is virtual. + + :param name: The name of the joint. + :return: True if the joint is virtual, False otherwise. + """ + return name in self.virtual_joint_names + @abstractmethod def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: """ - Adds a joint to this object. + Add a joint to this object. :param name: The name of the joint. :param child: The name of the child link. @@ -618,7 +650,7 @@ def add_joint(self, name: str, child: str, joint_type: JointType, def update_description_from_file(self, path: str) -> None: """ - Updates the description of this object from the file at the given path. + Update the description of this object from the file at the given path. :param path: The path of the file to update from. """ @@ -641,7 +673,7 @@ def parsed_description(self, parsed_description: Any): @abstractmethod def load_description(self, path: str) -> Any: """ - Loads the description from the file at the given path. + Load the description from the file at the given path. :param path: The path to the source file, if only a filename is provided then the resources directories will be searched. @@ -650,7 +682,7 @@ def load_description(self, path: str) -> Any: def generate_description_from_file(self, path: str, name: str, extension: str) -> str: """ - Generates and preprocesses the description from the file at the given path and returns the preprocessed + Generate and preprocess the description from the file at the given path and return the preprocessed description as a string. :param path: The path of the file to preprocess. @@ -679,7 +711,7 @@ def generate_description_from_file(self, path: str, name: str, extension: str) - def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: """ - Returns the file name of the description file. + Return the file name of the description file. :param path_object: The path object of the description file or the mesh file. :param extension: The file extension of the description file or the mesh file. @@ -699,8 +731,8 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @abstractmethod def generate_from_mesh_file(cls, path: str, name: str) -> str: """ - Generates a description file from one of the mesh types defined in the mesh_extensions and - returns the path of the generated file. + Generate a description file from one of the mesh types defined in the mesh_extensions and + return the path of the generated file. :param path: The path to the .obj file. :param name: The name of the object. @@ -710,11 +742,12 @@ def generate_from_mesh_file(cls, path: str, name: str) -> str: @classmethod @abstractmethod - def generate_from_description_file(cls, path: str) -> str: + def generate_from_description_file(cls, path: str, make_mesh_paths_absolute: bool = True) -> str: """ - Preprocesses the given file and returns the preprocessed description string. + Preprocess the given file and return the preprocessed description string. :param path: The path of the file to preprocess. + :param make_mesh_paths_absolute: Whether to make the mesh paths absolute. :return: The preprocessed description string. """ pass @@ -723,7 +756,7 @@ def generate_from_description_file(cls, path: str) -> str: @abstractmethod def generate_from_parameter_server(cls, name: str) -> str: """ - Preprocesses the description from the ROS parameter server and returns the preprocessed description string. + Preprocess the description from the ROS parameter server and return the preprocessed description string. :param name: The name of the description on the parameter server. :return: The preprocessed description string. diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 7288df776..e8a907bab 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -1,5 +1,6 @@ +import os import pathlib -from xml.etree import ElementTree +import xml.etree.ElementTree as ET import rospkg import rospy @@ -204,8 +205,8 @@ def load_description(self, path) -> URDF: def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: """ - Generates an URDf file with the given .obj or .stl file as mesh. In addition, the given rgba_color will be - used to create a material tag in the URDF. + Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a + material tag in the URDF. :param path: The path to the mesh file. :param name: The name of the object. @@ -237,21 +238,23 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) return content - def generate_from_description_file(self, path: str) -> str: + def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bool = True) -> str: with open(path, mode="r") as f: urdf_string = self.fix_missing_inertial(f.read()) urdf_string = self.remove_error_tags(urdf_string) urdf_string = self.fix_link_attributes(urdf_string) try: - urdf_string = self.correct_urdf_string(urdf_string) + urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) except rospkg.ResourceNotFound as e: rospy.logerr(f"Could not find resource package linked in this URDF") raise e - return urdf_string + return self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string def generate_from_parameter_server(self, name: str) -> str: urdf_string = rospy.get_param(name) - return self.correct_urdf_string(urdf_string) + urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + return self.fix_missing_inertial(urdf_string) def get_link_by_name(self, link_name: str) -> LinkDescription: """ @@ -309,9 +312,10 @@ def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: """ return self.parsed_description.get_chain(start_link_name, end_link_name) - def correct_urdf_string(self, urdf_string: str) -> str: + @staticmethod + def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: """ - Changes paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) + Change paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) can't deal with ROS package paths. :param urdf_string: The name of the URDf on the parameter server @@ -327,7 +331,26 @@ def correct_urdf_string(self, urdf_string: str) -> str: line = line.replace("package://" + s1[0], path) new_urdf_string += line + '\n' - return self.fix_missing_inertial(new_urdf_string) + return new_urdf_string + + @staticmethod + def make_mesh_paths_absolute(urdf_string: str, urdf_file_path: str) -> str: + # Parse the URDF file + root = ET.fromstring(urdf_string) + + # Iterate through all mesh tags + for mesh in root.findall('.//mesh'): + filename = mesh.attrib.get('filename', '') + if filename: + # If the filename is a relative path, convert it to an absolute path + if not os.path.isabs(filename): + # Deduce the base path from the relative path + base_path = os.path.dirname( + os.path.abspath(os.path.join(os.path.dirname(urdf_file_path), filename))) + abs_path = os.path.abspath(os.path.join(base_path, os.path.basename(filename))) + mesh.set('filename', abs_path) + + return ET.tostring(root, encoding='unicode') @staticmethod def fix_missing_inertial(urdf_string: str) -> str: @@ -339,10 +362,10 @@ def fix_missing_inertial(urdf_string: str) -> str: :returns: The new, corrected URDF description as string. """ - inertia_tree = ElementTree.ElementTree(ElementTree.Element("inertial")) - inertia_tree.getroot().append(ElementTree.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(ElementTree.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(ElementTree.Element("inertia", {"ixx": "0.01", + inertia_tree = ET.ElementTree(ET.Element("inertial")) + inertia_tree.getroot().append(ET.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(ET.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(ET.Element("inertia", {"ixx": "0.01", "ixy": "0", "ixz": "0", "iyy": "0.01", @@ -350,14 +373,14 @@ def fix_missing_inertial(urdf_string: str) -> str: "izz": "0.01"})) # create tree from string - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) for link_element in tree.iter("link"): inertial = [*link_element.iter("inertial")] if len(inertial) == 0: link_element.append(inertia_tree.getroot()) - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def remove_error_tags(urdf_string: str) -> str: @@ -368,14 +391,14 @@ def remove_error_tags(urdf_string: str) -> str: :param urdf_string: String of the URDF from which the tags should be removed :return: The URDF string with the tags removed """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) removing_tags = ["gazebo", "transmission"] for tag_name in removing_tags: all_tags = tree.findall(tag_name) for tag in all_tags: tree.getroot().remove(tag) - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def fix_link_attributes(urdf_string: str) -> str: @@ -385,13 +408,13 @@ def fix_link_attributes(urdf_string: str) -> str: :param urdf_string: The string of the URDF from which the attributes should be removed :return: The URDF string with the attributes removed """ - tree = ElementTree.ElementTree(ElementTree.fromstring(urdf_string)) + tree = ET.ElementTree(ET.fromstring(urdf_string)) for link in tree.iter("link"): if "type" in link.attrib.keys(): del link.attrib["type"] - return ElementTree.tostring(tree.getroot(), encoding='unicode') + return ET.tostring(tree.getroot(), encoding='unicode') @staticmethod def get_file_extension() -> str: diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 95549d85e..5ae0f1620 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,19 +1,18 @@ +import os from pathlib import Path from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.enums import ObjectType from pycram.world_concepts.world_object import Object -import pathlib class TestCacheManager(BulletWorldTestCase): def test_generate_description_and_write_to_cache(self): cache_manager = self.world.cache_manager - file_path = pathlib.Path(__file__).parent.resolve() - path = str(file_path) + "/../resources/apartment.urdf" + path = os.path.join(self.world.resources_path, "objects/apartment.urdf") extension = Path(path).suffix - cache_path = self.world.cache_dir + "/apartment.urdf" + cache_path = os.path.join(self.world.cache_dir, "apartment.urdf") apartment = Object("apartment", ObjectType.ENVIRONMENT, path) cache_manager.generate_description_and_write_to_cache(path, apartment.name, extension, cache_path, apartment.description) From b22192652383c52f5f8a6a78befe7a01d4769070 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 30 Jul 2024 19:16:35 +0200 Subject: [PATCH 091/189] [AbstractWorld] removed unnecessary sleep when acquiring and releasing object lock. --- src/pycram/datastructures/world.py | 2 -- src/pycram/ros/viz_marker_publisher.py | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index cbdbb1511..a701fd96c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -355,8 +355,6 @@ def remove_object(self, obj: Object) -> None: :param obj: The object to be removed. """ - while self.object_lock.locked(): - time.sleep(0.1) self.object_lock.acquire() obj.detach_all() diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 1aa2b0e09..7b7eca5f5 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -44,8 +44,6 @@ def _publish(self) -> None: Constantly publishes the Marker Array. To the given topic name at a fixed rate. """ while not self.kill_event.is_set(): - while self.lock.locked(): - time.sleep(0.1) self.lock.acquire() marker_array = self._make_marker_array() self.lock.release() From 0bfaeda5e01166826a052c2974ebf8e22d44f410 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 30 Jul 2024 19:48:44 +0200 Subject: [PATCH 092/189] [AbstractWorld] Added exceptions file to define more meaningful exceptions specific to pycram. --- src/pycram/datastructures/world.py | 36 +++++++++++++----------------- src/pycram/exceptions.py | 14 ++++++++++++ 2 files changed, 29 insertions(+), 21 deletions(-) create mode 100644 src/pycram/exceptions.py diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a701fd96c..5c111e68b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1,7 +1,6 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import os import threading import time from abc import ABC, abstractmethod @@ -24,6 +23,7 @@ ContactPointsList, VirtualMoveBaseJoints) from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform +from ..exceptions import ProspectionObjectNotFound, WorldObjectNotFound from ..local_transformer import LocalTransformer from ..robot_description import RobotDescription from ..world_concepts.constraints import Constraint @@ -915,16 +915,7 @@ def get_prospection_object_for_object(self, obj: Object) -> Object: :return: The corresponding object in the prospection world. """ with UseProspectionWorld(): - try: - if obj.world.is_prospection_world: - return obj - else: - return self.world_sync.get_prospection_object(obj) - except KeyError: - raise KeyError( - f"There is no prospection object for the given object: {obj.name}, this could be the case if" - f" the object isn't anymore in the main (graphical) World" - f" or if the given object is already a prospection object. ") + return self.world_sync.get_prospection_object(obj) def get_object_for_prospection_object(self, prospection_object: Object) -> Object: """ @@ -935,14 +926,7 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. """ - with UseProspectionWorld(): - try: - if not prospection_object.world.is_prospection_world: - return prospection_object - else: - return self.world_sync.get_world_object(prospection_object) - except KeyError: - raise KeyError(f"The given object {prospection_object.name} is not in the prospection world.") + return self.world_sync.get_world_object(prospection_object) def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: """ @@ -1290,7 +1274,12 @@ def get_world_object(self, prospection_object: Object) -> Object: :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. """ - return self.prospection_object_to_object_map[prospection_object] + try: + return self.prospection_object_to_object_map[prospection_object] + except KeyError: + if prospection_object in self.world.objects: + return prospection_object + raise WorldObjectNotFound(prospection_object) def get_prospection_object(self, obj: Object) -> Object: """ @@ -1299,7 +1288,12 @@ def get_prospection_object(self, obj: Object) -> Object: :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. """ - return self.object_to_prospection_object_map[obj] + try: + return self.object_to_prospection_object_map[obj] + except KeyError: + if obj in self.prospection_world.objects: + return obj + raise ProspectionObjectNotFound(obj) def sync_worlds(self): """ diff --git a/src/pycram/exceptions.py b/src/pycram/exceptions.py new file mode 100644 index 000000000..b56f7fb5f --- /dev/null +++ b/src/pycram/exceptions.py @@ -0,0 +1,14 @@ +from typing_extensions import TYPE_CHECKING + +if TYPE_CHECKING: + from pycram.world_concepts.world_object import Object + + +class ProspectionObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the prospection world.") + + +class WorldObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the main world.") From 1f78c4488b47d6fafe49c4b9baa23d610f9c6b0f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 30 Jul 2024 20:13:44 +0200 Subject: [PATCH 093/189] [AbstractWorld] Add new exception for failed generation of object description from given path. --- src/pycram/datastructures/world.py | 2 +- src/pycram/description.py | 8 ++++---- src/pycram/exceptions.py | 11 +++++++++++ src/pycram/world_concepts/world_object.py | 24 +++++++++++++---------- 4 files changed, 30 insertions(+), 15 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 5c111e68b..713408505 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1197,7 +1197,7 @@ class UseProspectionWorld: with UseProspectionWorld(): NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform() """ - WAIT_TIME_AS_N_SIMULATION_STEPS = 20 + WAIT_TIME_AS_N_SIMULATION_STEPS: int = 20 """ The time in simulation steps to wait before switching to the prospection world """ diff --git a/src/pycram/description.py b/src/pycram/description.py index d9ca6a829..8e8f5e57c 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -12,6 +12,7 @@ from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world import WorldEntity +from .exceptions import ObjectDescriptionNotFound from .local_transformer import LocalTransformer if TYPE_CHECKING: @@ -701,11 +702,10 @@ def generate_description_from_file(self, path: str, name: str, extension: str) - # Using the description from the parameter server description_string = self.generate_from_parameter_server(path) except KeyError: - logging.warning(f"Couldn't find dile data in the ROS parameter server") + logging.warning(f"Couldn't find file data in the ROS parameter server") + if description_string is None: - logging.error(f"Could not find file with path {path} in the resources directory nor" - f" in the ros parameter server.") - raise FileNotFoundError + raise ObjectDescriptionNotFound(name, path, extension) return description_string diff --git a/src/pycram/exceptions.py b/src/pycram/exceptions.py index b56f7fb5f..37666b398 100644 --- a/src/pycram/exceptions.py +++ b/src/pycram/exceptions.py @@ -12,3 +12,14 @@ def __init__(self, obj: 'Object'): class WorldObjectNotFound(KeyError): def __init__(self, obj: 'Object'): super().__init__(f"The given object {obj.name} is not in the main world.") + + +class ObjectAlreadyExists(Exception): + def __init__(self, obj: 'Object'): + super().__init__(f"An object with the name {obj.name} already exists in the world.") + + +class ObjectDescriptionNotFound(KeyError): + def __init__(self, object_name: str, path: str, extension: str): + super().__init__(f"{object_name} with path {path} and extension {extension} is not in supported extensions, and" + f" the description data was not found on the ROS parameter server") diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 11405ee4c..fe2be5371 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -9,6 +9,7 @@ from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..description import ObjectDescription, LinkDescription, Joint, JointDescription +from ..exceptions import ObjectAlreadyExists from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..datastructures.world import WorldEntity, World from ..world_concepts.constraints import Attachment @@ -58,11 +59,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, super().__init__(-1, world) - if pose is None: - pose = Pose() - if name in [obj.name for obj in self.world.objects]: - rospy.logerr(f"An object with the name {name} already exists in the world.") - raise ValueError(f"An object with the name {name} already exists in the world.") + pose = Pose() if pose is None else pose + self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color @@ -92,6 +90,16 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + @property + def name(self): + return self._name + + @name.setter + def name(self, name: str): + self._name = name + if name in [obj.name for obj in self.world.objects]: + raise ObjectAlreadyExists(self) + @property def pose(self): return self.get_pose() @@ -119,11 +127,7 @@ def _load_object_and_get_id(self, path: str, :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path of the file that was loaded. """ - try: - self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) - except FileNotFoundError as e: - logging.error("Could not generate description from file.") - raise e + self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) try: path = self.path if self.world.let_pycram_handle_spawning else self.name From 81d78546e64e1690d863b9f43189c1cec4435154 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 13:00:36 +0200 Subject: [PATCH 094/189] [AbstractWorld] Moved joint type mapping for multiverse to the enum class definition, and defined a new exception for unsupported joint types, and defined a multiverse class variable as a tuple providing the supported joint types. --- src/pycram/worlds/multiverse.py | 17 ++++++----------- .../worlds/multiverse_datastructures/enums.py | 13 ++++++++++++- .../worlds/multiverse_functions/exceptions.py | 6 ++++++ test/test_multiverse.py | 8 +++++--- 4 files changed, 29 insertions(+), 15 deletions(-) create mode 100644 src/pycram/worlds/multiverse_functions/exceptions.py diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a8bad45d2..e663684a3 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -24,12 +24,9 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ - _joint_type_to_position_name: Dict[JointType, MultiverseJointProperty] = { - JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_POSITION, - JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_POSITION, - } + supported_joint_types = (JointType.REVOLUTE, JointType.PRISMATIC) """ - A dictionary to map JointType to the corresponding multiverse attribute name. + A Tuple for the supported pycram joint types in Multiverse. """ added_multiverse_resources: bool = False @@ -139,10 +136,9 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) - def get_joint_position_name(self, joint: Joint) -> MultiverseJointProperty: - if joint.type not in self._joint_type_to_position_name: - raise ValueError(f"Joint type {joint.type} is not supported in Multiverse") - return self._joint_type_to_position_name[joint.type] + @staticmethod + def get_joint_position_name(joint: Joint) -> MultiverseJointProperty: + return MultiverseJointProperty.from_pycram_joint_type(joint.type) def load_object_and_get_id(self, name: Optional[str] = None, pose: Optional[Pose] = None, @@ -174,8 +170,7 @@ def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: return self.last_object_id def get_object_joint_names(self, obj: Object) -> List[str]: - return [joint.name for joint in obj.description.joints - if joint.type in self._joint_type_to_position_name.keys()] + return [joint.name for joint in obj.description.joints if joint.type in self.supported_joint_types] def get_object_link_names(self, obj: Object) -> List[str]: return [link.name for link in obj.description.links] diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index cb001a789..12a943637 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -1,6 +1,8 @@ -from abc import ABC from enum import Enum +from pycram.datastructures.enums import JointType +from ..multiverse_functions.exceptions import UnsupportedJointType + class MultiverseAPIName(Enum): """ @@ -36,3 +38,12 @@ class MultiverseJointProperty(MultiverseProperty): PRISMATIC_JOINT_POSITION = "joint_tvalue" REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointProperty': + if joint_type == JointType.REVOLUTE: + return MultiverseJointProperty.REVOLUTE_JOINT_POSITION + elif joint_type == JointType.PRISMATIC: + return MultiverseJointProperty.PRISMATIC_JOINT_POSITION + else: + raise UnsupportedJointType(joint_type) \ No newline at end of file diff --git a/src/pycram/worlds/multiverse_functions/exceptions.py b/src/pycram/worlds/multiverse_functions/exceptions.py new file mode 100644 index 000000000..bca03b173 --- /dev/null +++ b/src/pycram/worlds/multiverse_functions/exceptions.py @@ -0,0 +1,6 @@ +from pycram.datastructures.enums import JointType + + +class UnsupportedJointType(Exception): + def __init__(self, joint_type: 'JointType'): + super().__init__(f"Unsupported joint type: {joint_type}") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 91bdc52d2..c5509bd8f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +import time import unittest import numpy as np @@ -130,7 +131,7 @@ def test_set_joint_position(self): original_joint_position = robot.get_joint_position(joint) robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.01) + self.assertAlmostEqual(joint_position, original_joint_position + step, delta=0.05) def test_spawn_robot(self): if self.multiverse.robot is not None: @@ -164,6 +165,7 @@ def test_set_robot_position(self): robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], delta=0.2) self.tearDown() + time.sleep(0.5) def test_set_robot_orientation(self): self.spawn_robot() @@ -207,8 +209,8 @@ def test_detach_object(self): milk.set_position(milk_position) new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.005) - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.01) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.02) self.tearDown() def test_attach_with_robot(self): From 09b48072f0bb7d8d63f5a066a4758f0d68d7a989 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 13:24:59 +0200 Subject: [PATCH 095/189] [Multiverse] Use datetime instead of float. --- .../multiverse_communication/clients.py | 21 ++++++++----------- test/test_multiverse.py | 6 +++--- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index b668522ba..e3d5b47fa 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -1,3 +1,4 @@ +import datetime import logging import threading from time import time, sleep @@ -31,7 +32,7 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: Optional[float] = 1 + MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) """ The maximum wait time for the data in seconds. """ @@ -128,7 +129,7 @@ def wait_for_body_data(self, name: str, properties: Optional[List[str]] = None) return: The body data as a dictionary. """ start = time() - while time() - start < self.MAX_WAIT_TIME_FOR_DATA: + while time() - start < self.MAX_WAIT_TIME_FOR_DATA.total_seconds(): received_data = self.get_received_data() data_received_flag = self.check_for_body_data(name, received_data, properties) if data_received_flag: @@ -175,11 +176,11 @@ def join(self): class MultiverseWriter(MultiverseClient): - time_for_sim_update: Optional[float] = 0.02 + TIME_FOR_SIMULATION_TO_UPDATE: datetime.timedelta = datetime.timedelta(milliseconds=20) """ Wait time for the sent data to be applied in the simulator. """ - time_for_setting_body_data: Optional[float] = 0.01 + TIME_FOR_SETTING_BODY_DATA: datetime.timedelta = datetime.timedelta(milliseconds=10) """ Wait time for setting body data. """ @@ -272,7 +273,7 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[str, List[ for value in data] self.send_data = [time() - self.time_start, *flattened_data] self.send_and_receive_data() - sleep(self.time_for_setting_body_data) + sleep(self.TIME_FOR_SETTING_BODY_DATA.total_seconds()) return self.response_meta_data def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: @@ -304,17 +305,13 @@ def send_data_to_server(self, data: List, self.send_and_receive_meta_data() self.send_data = data self.send_and_receive_data() - sleep(self.time_for_sim_update) + sleep(self.TIME_FOR_SIMULATION_TO_UPDATE.total_seconds()) return self.response_meta_data class MultiverseAPI(MultiverseClient): - BASE_NAME: str = "api_requester" - """ - The base name of the Multiverse reader. - """ - API_REQUEST_WAIT_TIME: float = 0.2 + API_REQUEST_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=200) """ The wait time for the API request in seconds. """ @@ -534,7 +531,7 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s self._add_api_request(api_name.value, *params) self._send_api_request() responses = self._get_all_apis_responses() - sleep(self.API_REQUEST_WAIT_TIME) + sleep(self.API_REQUEST_WAIT_TIME.total_seconds()) return responses def _get_all_apis_responses(self) -> Dict[API, List[str]]: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index c904a976a..9c4e80b37 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -177,7 +177,7 @@ def test_attach_object(self): estimated_cup_position[0] += 1 milk.set_position(milk_position) new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2]) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.06) # @unittest.skip("Not implemented feature yet.") def test_detach_object(self): @@ -195,8 +195,8 @@ def test_detach_object(self): milk.set_position(milk_position) new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.005) - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.002) + self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], 0.05) + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], 0.05) self.tearDown() def test_attach_with_robot(self): From 974cde3f6c200a50422925e9e57784fa7391e3ea Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 15:30:58 +0200 Subject: [PATCH 096/189] [AbstractWorld] Some cleaning. --- src/pycram/description.py | 8 +- src/pycram/external_interfaces/giskard.py | 2 +- src/pycram/world_concepts/world_object.py | 241 +++++++++++----------- 3 files changed, 127 insertions(+), 124 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 8e8f5e57c..3b46bc85a 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -204,7 +204,7 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): LinkDescription.__init__(self, link_description.parsed_description) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} - self._update_pose() + self.update_pose() def set_pose(self, pose: Pose) -> None: """ @@ -369,7 +369,7 @@ def orientation_as_list(self) -> List[float]: """ return self.pose.orientation_as_list() - def _update_pose(self) -> None: + def update_pose(self) -> None: """ Update the current pose of this link from the world. """ @@ -383,7 +383,7 @@ def pose(self) -> Pose: :return: A Pose object containing the pose of the link relative to the world frame. """ if self.world.update_poses_from_sim_on_get: - self._update_pose() + self.update_pose() return self._current_pose @property @@ -459,7 +459,7 @@ def tf_frame(self) -> str: """ return self.object.tf_frame - def _update_pose(self) -> None: + def update_pose(self) -> None: self._current_pose = self.world.get_object_pose(self.object) def __copy__(self): diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 925584fa5..02cd07fba 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -169,7 +169,7 @@ def spawn_object(object: Object) -> None: :param object: World object that should be spawned """ if len(object.link_name_to_id) == 1: - geometry = object.get_link_geometry(object.root_link_name) + geometry = object.get_link_geometry(object.root_link.name) if isinstance(geometry, MeshVisualShape): filename = geometry.file_name spawn_mesh(object.name, filename, object.get_pose()) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index fe2be5371..3860d258f 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,7 +8,7 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from ..description import ObjectDescription, LinkDescription, Joint, JointDescription +from ..description import ObjectDescription, LinkDescription, Joint from ..exceptions import ObjectAlreadyExists from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..datastructures.world import WorldEntity, World @@ -35,11 +35,11 @@ class Object(WorldEntity): """ def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Optional[Type[ObjectDescription]] = URDFObject, + description: Type[ObjectDescription] = URDFObject, pose: Optional[Pose] = None, world: Optional[World] = None, - color: Optional[Color] = Color(), - ignore_cached_files: Optional[bool] = False): + color: Color = Color(), + ignore_cached_files: bool = False): """ The constructor loads the description file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -52,7 +52,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, searched. :param description: The ObjectDescription of the object, this contains the joints and links of the object. :param pose: The pose at which the Object should be spawned - :param world: The World in which the object should be spawned, if no world is specified the :py:attr:`~World.current_world` will be used. + :param world: The World in which the object should be spawned, if no world is specified the + :py:attr:`~World.current_world` will be used. :param color: The rgba_color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. """ @@ -113,7 +114,7 @@ def transform(self): return self.get_pose().to_transform(self.tf_frame) def _load_object_and_get_id(self, path: str, - ignore_cached_files: Optional[bool] = False) -> Tuple[int, Union[str, None]]: + ignore_cached_files: bool = False) -> Tuple[int, Union[str, None]]: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -121,7 +122,7 @@ def _load_object_and_get_id(self, path: str, and this URDf file will be loaded instead. When spawning a URDf file a new file will be created in the cache directory, if there exists none. This new file will have resolved mesh file paths, meaning there will be no references - to ROS packges instead there will be absolute file paths. + to ROS packages instead there will be absolute file paths. :param path: The path to the description file. :param ignore_cached_files: Whether to ignore files in the cache directory. @@ -143,8 +144,8 @@ def _load_object_and_get_id(self, path: str, def _update_world_robot_and_description(self): """ - Initializes the robot description of the object, loads the description from the RobotDescriptionManager and sets - the robot as the current robot in the World. Also adds the virtual move base joints to the robot. + Initialize the robot description of the object, load the description from the RobotDescriptionManager and set + the robot as the current robot in the World. Also add the virtual move base joints to the robot. """ rdm = RobotDescriptionManager() rdm.load_description(self.name) @@ -152,17 +153,20 @@ def _update_world_robot_and_description(self): self._add_virtual_move_base_joints() def _add_virtual_move_base_joints(self): + """ + Add the virtual move base joints to the robot description. + """ virtual_joints = RobotDescription.current_robot_description.virtual_move_base_joints if virtual_joints is None: return - child_link = self.root_link_name + child_link = self.description.get_root() axes = virtual_joints.get_axes() for joint_name, joint_type in virtual_joints.get_types().items(): self.description.add_joint(joint_name, child_link, joint_type, axes[joint_name]) def _init_joint_name_and_id_map(self) -> None: """ - Creates a dictionary which maps the joint names to their unique ids and vice versa. + Create a dictionary which maps the joint names to their unique ids and vice versa. """ n_joints = len(self.joint_names) self.joint_name_to_id = dict(zip(self.joint_names, range(n_joints))) @@ -170,22 +174,22 @@ def _init_joint_name_and_id_map(self) -> None: def _init_link_name_and_id_map(self) -> None: """ - Creates a dictionary which maps the link names to their unique ids and vice versa. + Create a dictionary which maps the link names to their unique ids and vice versa. """ n_links = len(self.link_names) self.link_name_to_id: Dict[str, int] = dict(zip(self.link_names, range(n_links))) - self.link_name_to_id[self.root_link_name] = -1 + self.link_name_to_id[self.description.get_root()] = -1 self.link_id_to_name: Dict[int, str] = dict(zip(self.link_name_to_id.values(), self.link_name_to_id.keys())) def _init_links_and_update_transforms(self) -> None: """ - Initializes the link objects from the URDF file and creates a dictionary which maps the link names to the + Initialize the link objects from the URDF file and creates a dictionary which maps the link names to the corresponding link objects. """ self.links = {} for link_name, link_id in self.link_name_to_id.items(): link_description = self.description.get_link_by_name(link_name) - if link_name == self.root_link_name: + if link_name == self.description.get_root(): self.links[link_name] = self.description.RootLink(self) else: self.links[link_name] = self.description.Link(link_id, link_description, self) @@ -205,7 +209,7 @@ def _init_joints(self): @property def has_one_link(self) -> bool: """ - Returns True if the object has only one link, otherwise False. + Return True if the object has only one link, otherwise False. """ return len(self.links) == 1 @@ -225,7 +229,7 @@ def joint_names(self) -> List[str]: def get_link(self, link_name: str) -> ObjectDescription.Link: """ - Returns the link object with the given name. + Return the link object with the given name. :param link_name: The name of the link. :return: The link object. @@ -234,7 +238,7 @@ def get_link(self, link_name: str) -> ObjectDescription.Link: def get_link_pose(self, link_name: str) -> Pose: """ - Returns the pose of the link with the given name. + Return the pose of the link with the given name. :param link_name: The name of the link. :return: The pose of the link. @@ -243,7 +247,7 @@ def get_link_pose(self, link_name: str) -> Pose: def get_link_position(self, link_name: str) -> Point: """ - Returns the position of the link with the given name. + Return the position of the link with the given name. :param link_name: The name of the link. :return: The position of the link. @@ -252,7 +256,7 @@ def get_link_position(self, link_name: str) -> Point: def get_link_position_as_list(self, link_name: str) -> List[float]: """ - Returns the position of the link with the given name. + Return the position of the link with the given name. :param link_name: The name of the link. :return: The position of the link. @@ -261,7 +265,7 @@ def get_link_position_as_list(self, link_name: str) -> List[float]: def get_link_orientation(self, link_name: str) -> Quaternion: """ - Returns the orientation of the link with the given name. + Return the orientation of the link with the given name. :param link_name: The name of the link. :return: The orientation of the link. @@ -270,7 +274,7 @@ def get_link_orientation(self, link_name: str) -> Quaternion: def get_link_orientation_as_list(self, link_name: str) -> List[float]: """ - Returns the orientation of the link with the given name. + Return the orientation of the link with the given name. :param link_name: The name of the link. :return: The orientation of the link. @@ -279,7 +283,7 @@ def get_link_orientation_as_list(self, link_name: str) -> List[float]: def get_link_tf_frame(self, link_name: str) -> str: """ - Returns the tf frame of the link with the given name. + Return the tf frame of the link with the given name. :param link_name: The name of the link. :return: The tf frame of the link. @@ -288,7 +292,7 @@ def get_link_tf_frame(self, link_name: str) -> str: def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of the link with the given name. + Return the axis aligned bounding box of the link with the given name. :param link_name: The name of the link. :return: The axis aligned bounding box of the link. @@ -297,7 +301,7 @@ def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBound def get_transform_between_links(self, from_link: str, to_link: str) -> Transform: """ - Returns the transform between two links. + Return the transform between two links. :param from_link: The name of the link from which the transform should be calculated. :param to_link: The name of the link to which the transform should be calculated. @@ -306,7 +310,7 @@ def get_transform_between_links(self, from_link: str, to_link: str) -> Transform def get_link_color(self, link_name: str) -> Color: """ - Returns the color of the link with the given name. + Return the color of the link with the given name. :param link_name: The name of the link. :return: The color of the link. @@ -315,7 +319,7 @@ def get_link_color(self, link_name: str) -> Color: def set_link_color(self, link_name: str, color: List[float]) -> None: """ - Sets the color of the link with the given name. + Set the color of the link with the given name. :param link_name: The name of the link. :param color: The new color of the link. @@ -324,7 +328,7 @@ def set_link_color(self, link_name: str, color: List[float]) -> None: def get_link_geometry(self, link_name: str) -> Union[VisualShape, None]: """ - Returns the geometry of the link with the given name. + Return the geometry of the link with the given name. :param link_name: The name of the link. :return: The geometry of the link. @@ -333,7 +337,7 @@ def get_link_geometry(self, link_name: str) -> Union[VisualShape, None]: def get_link_transform(self, link_name: str) -> Transform: """ - Returns the transform of the link with the given name. + Return the transform of the link with the given name. :param link_name: The name of the link. :return: The transform of the link. @@ -342,7 +346,7 @@ def get_link_transform(self, link_name: str) -> Transform: def get_link_origin(self, link_name: str) -> Pose: """ - Returns the origin of the link with the given name. + Return the origin of the link with the given name. :param link_name: The name of the link. :return: The origin of the link as a 'Pose'. @@ -351,7 +355,7 @@ def get_link_origin(self, link_name: str) -> Pose: def get_link_origin_transform(self, link_name: str) -> Transform: """ - Returns the origin transform of the link with the given name. + Return the origin transform of the link with the given name. :param link_name: The name of the link. :return: The origin transform of the link. @@ -374,16 +378,16 @@ def __repr__(self): def remove(self) -> None: """ - Removes this object from the World it currently resides in. + Remove this object from the World it currently resides in. For the object to be removed it has to be detached from all objects it - is currently attached to. After this is done a call to world remove object is done + is currently attached to. After this call world remove object to remove this Object from the simulation/world. """ self.world.remove_object(self) def reset(self, remove_saved_states=True) -> None: """ - Resets the Object to the state it was first spawned in. + Reset the Object to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and the object will be set to the position and orientation in which it was spawned. @@ -400,10 +404,10 @@ def attach(self, child_object: Object, parent_link: Optional[str] = None, child_link: Optional[str] = None, - bidirectional: Optional[bool] = True, - coincide_the_objects: Optional[bool] = False) -> None: + bidirectional: bool = True, + coincide_the_objects: bool = False) -> None: """ - Attaches another object to this object. This is done by + Attach another object to this object. This is done by saving the transformation between the given link, if there is one, and the base pose of the other object. Additionally, the name of the link, to which the object is attached, will be saved. @@ -434,7 +438,7 @@ def attach(self, def detach(self, child_object: Object) -> None: """ - Detaches another object from this object. This is done by + Detache another object from this object. This is done by deleting the attachment from the attachments dictionary of both objects and deleting the constraint of the simulator. Afterward the detachment event of the corresponding World will be fired. @@ -459,7 +463,7 @@ def update_attachment_with_object(self, child_object: Object): def get_position(self) -> Point: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -467,7 +471,7 @@ def get_position(self) -> Point: def get_orientation(self) -> Pose.orientation: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + Return the orientation of this object as a list of xyzw, representing a quaternion. :return: A list of xyzw """ @@ -475,7 +479,7 @@ def get_orientation(self) -> Pose.orientation: def get_position_as_list(self) -> List[float]: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -483,7 +487,7 @@ def get_position_as_list(self) -> List[float]: def get_base_position_as_list(self) -> List[float]: """ - Returns the position of this Object as a list of xyz. + Return the position of this Object as a list of xyz. :return: The current position of this object """ @@ -491,7 +495,7 @@ def get_base_position_as_list(self) -> List[float]: def get_orientation_as_list(self) -> List[float]: """ - Returns the orientation of this object as a list of xyzw, representing a quaternion. + Return the orientation of this object as a list of xyzw, representing a quaternion. :return: A list of xyzw """ @@ -499,7 +503,7 @@ def get_orientation_as_list(self) -> List[float]: def get_pose(self) -> Pose: """ - Returns the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. + Return the position of this object as a list of xyz. Alias for :func:`~Object.get_position`. :return: The current pose of this object """ @@ -507,9 +511,9 @@ def get_pose(self) -> Pose: self.update_pose() return self._current_pose - def set_pose(self, pose: Pose, base: Optional[bool] = False, set_attachments: Optional[bool] = True) -> None: + def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True) -> None: """ - Sets the Pose of the object. + Set the Pose of the object. :param pose: New Pose for the object :param base: If True places the object base instead of origin at the specified position and orientation @@ -530,7 +534,7 @@ def reset_base_pose(self, pose: Pose): def update_pose(self): """ - Updates the current pose of this object from the world, and updates the poses of all links. + Update the current pose of this object from the world, and updates the poses of all links. """ self._current_pose = self.world.get_object_pose(self) # TODO: Probably not needed, need to test @@ -539,10 +543,10 @@ def update_pose(self): def _update_all_links_poses(self): """ - Updates the poses of all links by getting them from the simulator. + Update the poses of all links by getting them from the simulator. """ for link in self.links.values(): - link._update_pose() + link.update_pose() def move_base_to_origin_pose(self) -> None: """ @@ -553,7 +557,7 @@ def move_base_to_origin_pose(self) -> None: def save_state(self, state_id) -> None: """ - Saves the state of this object by saving the state of all links and attachments. + Save the state of this object by saving the state of all links and attachments. :param state_id: The unique id of the state. """ @@ -563,7 +567,7 @@ def save_state(self, state_id) -> None: def save_links_states(self, state_id: int) -> None: """ - Saves the state of all links of this object. + Save the state of all links of this object. :param state_id: The unique id of the state. """ @@ -572,7 +576,7 @@ def save_links_states(self, state_id: int) -> None: def save_joints_states(self, state_id: int) -> None: """ - Saves the state of all joints of this object. + Save the state of all joints of this object. :param state_id: The unique id of the state. """ @@ -581,10 +585,17 @@ def save_joints_states(self, state_id: int) -> None: @property def current_state(self) -> ObjectState: - return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), self.joint_states.copy()) + """ + The current state of this object as an ObjectState. + """ + return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), + self.joint_states.copy()) @current_state.setter def current_state(self, state: ObjectState) -> None: + """ + Set the current state of this object to the given state. + """ if self.get_pose().dist(state.pose) != 0.0: self.set_pose(state.pose, base=False, set_attachments=False) @@ -594,13 +605,13 @@ def current_state(self, state: ObjectState) -> None: def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ - Sets the attachments of this object to the given attachments. + Set the attachments of this object to the given attachments. :param attachments: A dictionary with the object as key and the attachment as value. """ for obj, attachment in attachments.items(): if self.world.is_prospection_world and not obj.world.is_prospection_world: - # In case this object is in the prospection world and the other object is not, the attachment will no + # In case this object is in the prospection world and the other object is not, the attachment will not # be set. continue if obj in self.attachments: @@ -614,7 +625,7 @@ def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: @property def link_states(self) -> Dict[int, LinkState]: """ - Returns the current state of all links of this object. + The current state of all links of this object. :return: A dictionary with the link id as key and the current state of the link as value. """ @@ -623,7 +634,7 @@ def link_states(self) -> Dict[int, LinkState]: @link_states.setter def link_states(self, link_states: Dict[int, LinkState]) -> None: """ - Sets the current state of all links of this object. + Set the current state of all links of this object. :param link_states: A dictionary with the link id as key and the current state of the link as value. """ @@ -633,7 +644,7 @@ def link_states(self, link_states: Dict[int, LinkState]) -> None: @property def joint_states(self) -> Dict[int, JointState]: """ - Returns the current state of all joints of this object. + The current state of all joints of this object. :return: A dictionary with the joint id as key and the current state of the joint as value. """ @@ -642,7 +653,7 @@ def joint_states(self) -> Dict[int, JointState]: @joint_states.setter def joint_states(self, joint_states: Dict[int, JointState]) -> None: """ - Sets the current state of all joints of this object. + Set the current state of all joints of this object. :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. """ @@ -651,7 +662,7 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: def remove_saved_states(self) -> None: """ - Removes all saved states of this object. + Remove all saved states of this object. """ super().remove_saved_states() self.remove_links_saved_states() @@ -659,27 +670,27 @@ def remove_saved_states(self) -> None: def remove_links_saved_states(self) -> None: """ - Removes all saved states of the links of this object. + Remove all saved states of the links of this object. """ for link in self.links.values(): link.remove_saved_states() def remove_joints_saved_states(self) -> None: """ - Removes all saved states of the joints of this object. + Remove all saved states of the joints of this object. """ for joint in self.joints.values(): joint.remove_saved_states() def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Object]] = None) -> None: """ - Updates the positions of all attached objects. This is done + Update the positions of all attached objects. This is done by calculating the new pose in world coordinate frame and setting the base pose of the attached objects to this new pose. - After this the _set_attached_objects method of all attached objects - will be called. + After this call _set_attached_objects method for all attached objects. - :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. + :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops + in the update. """ if not self.world.let_pycram_move_attached_objects: return @@ -704,7 +715,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec def set_position(self, position: Union[Pose, Point, List], base=False) -> None: """ - Sets this Object to the given position, if base is true the bottom of the Object will be placed at the position + Set this Object to the given position, if base is true, place the bottom of the Object at the position instead of the origin in the center of the Object. The given position can either be a Pose, in this case only the position is used or a geometry_msgs.msg/Point which is the position part of a Pose. @@ -731,7 +742,7 @@ def set_position(self, position: Union[Pose, Point, List], base=False) -> None: def set_orientation(self, orientation: Union[Pose, Quaternion, List, Tuple, np.ndarray]) -> None: """ - Sets the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only + Set the orientation of the Object to the given orientation. Orientation can either be a Pose, in this case only the orientation of this pose is used or a geometry_msgs.msg/Quaternion which is the orientation of a Pose. :param orientation: Target orientation given as a list of xyzw. @@ -754,7 +765,7 @@ def set_orientation(self, orientation: Union[Pose, Quaternion, List, Tuple, np.n def get_joint_id(self, name: str) -> int: """ - Returns the unique id for a joint name. As used by the world/simulator. + Return the unique id for a joint name. As used by the world/simulator. :param name: The joint name :return: The unique id @@ -763,61 +774,43 @@ def get_joint_id(self, name: str) -> int: def get_root_link_description(self) -> LinkDescription: """ - Returns the root link of the URDF of this object. + Return the root link of the URDF of this object. :return: The root link as defined in the URDF of this object. """ for link_description in self.description.links: - if link_description.name == self.root_link_name: + if link_description.name == self.description.get_root(): return link_description @property def root_link(self) -> ObjectDescription.Link: """ - Returns the root link of this object. + The root link of this object. :return: The root link of this object. """ - return self.links[self.root_link_name] + return self.links[self.description.get_root()] @property def tip_link(self) -> ObjectDescription.Link: """ - Returns the tip link of this object. + The tip link of this object. :return: The tip link of this object. """ - return self.links[self.tip_link_name] - - @property - def root_link_name(self) -> str: - """ - Returns the name of the root link of this object. - - :return: The name of the root link of this object. - """ - return self.description.get_root() - - @property - def tip_link_name(self) -> str: - """ - Returns the name of the tip link of this object. - - :return: The name of the tip link of this object. - """ - return self.description.get_tip() + return self.links[self.description.get_tip()] def get_root_link_id(self) -> int: """ - Returns the unique id of the root link of this object. + Return the unique id of the root link of this object. :return: The unique id of the root link of this object. """ - return self.get_link_id(self.root_link_name) + return self.get_link_id(self.description.get_root()) def get_link_id(self, link_name: str) -> int: """ - Returns a unique id for a link name. + Return a unique id for a link name. :param link_name: The name of the link. :return: The unique id of the link. @@ -826,7 +819,7 @@ def get_link_id(self, link_name: str) -> int: def get_link_by_id(self, link_id: int) -> ObjectDescription.Link: """ - Returns the link for a given unique link id + Return the link for a given unique link id :param link_id: The unique id of the link. :return: The link object. @@ -835,7 +828,7 @@ def get_link_by_id(self, link_id: int) -> ObjectDescription.Link: def reset_all_joints_positions(self) -> None: """ - Sets the current position of all joints to 0. This is useful if the joints should be reset to their default + Set the current position of all joints to 0. This is useful if the joints should be reset to their default """ joint_names = list(self.joint_name_to_id.keys()) if len(joint_names) == 0: @@ -845,7 +838,7 @@ def reset_all_joints_positions(self) -> None: def set_joint_positions(self, joint_poses: dict) -> None: """ - Sets the current position of multiple joints at once, this method should be preferred when setting + Set the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_poses: @@ -858,7 +851,7 @@ def set_joint_positions(self, joint_poses: dict) -> None: def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ - Sets the position of the given joint to the given joint pose and updates the poses of all attached objects. + Set the position of the given joint to the given joint pose and updates the poses of all attached objects. :param joint_name: The name of the joint :param joint_position: The target pose for this joint @@ -870,6 +863,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: def get_joint_position(self, joint_name: str) -> float: """ + Return the current position of the given joint. :param joint_name: The name of the joint :return: The current position of the given joint """ @@ -877,6 +871,7 @@ def get_joint_position(self, joint_name: str) -> float: def get_joint_damping(self, joint_name: str) -> float: """ + Return the damping of the given joint (friction). :param joint_name: The name of the joint :return: The damping of the given joint """ @@ -884,6 +879,7 @@ def get_joint_damping(self, joint_name: str) -> float: def get_joint_upper_limit(self, joint_name: str) -> float: """ + Return the upper limit of the given joint. :param joint_name: The name of the joint :return: The upper limit of the given joint """ @@ -891,6 +887,7 @@ def get_joint_upper_limit(self, joint_name: str) -> float: def get_joint_lower_limit(self, joint_name: str) -> float: """ + Return the lower limit of the given joint. :param joint_name: The name of the joint :return: The lower limit of the given joint """ @@ -898,6 +895,7 @@ def get_joint_lower_limit(self, joint_name: str) -> float: def get_joint_axis(self, joint_name: str) -> Point: """ + Return the axis of the given joint. :param joint_name: The name of the joint :return: The axis of the given joint """ @@ -905,6 +903,7 @@ def get_joint_axis(self, joint_name: str) -> Point: def get_joint_type(self, joint_name: str) -> JointType: """ + Return the type of the given joint. :param joint_name: The name of the joint :return: The type of the given joint """ @@ -912,6 +911,7 @@ def get_joint_type(self, joint_name: str) -> JointType: def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: """ + Return the lower and upper limits of the given joint. :param joint_name: The name of the joint :return: The lower and upper limits of the given joint """ @@ -919,6 +919,7 @@ def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: """ + Return the child link of the given joint. :param joint_name: The name of the joint :return: The child link of the given joint """ @@ -926,6 +927,8 @@ def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: def get_joint_parent_link(self, joint_name: str) -> ObjectDescription.Link: """ + + :param joint_name: The name of the joint :return: The parent link of the given joint """ @@ -933,13 +936,13 @@ def get_joint_parent_link(self, joint_name: str) -> ObjectDescription.Link: def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: """ - Traverses the chain from 'link' to the URDF origin and returns the first joint that is of type 'joint_type'. + Traverse the chain from 'link' to the URDF origin and return the first joint that is of type 'joint_type'. :param link_name: AbstractLink name above which the joint should be found :param joint_type: Joint type that should be searched for :return: Name of the first joint which has the given type """ - chain = self.description.get_chain(self.root_link_name, link_name) + chain = self.description.get_chain(self.description.get_root(), link_name) reversed_chain = reversed(chain) container_joint = None for element in reversed_chain: @@ -952,7 +955,7 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: def get_positions_of_all_joints(self) -> Dict[str, float]: """ - Returns the positions of all joints of the object as a dictionary of joint names and joint positions. + Return the positions of all joints of the object as a dictionary of joint names and joint positions. :return: A dictionary with all joints positions'. """ @@ -960,14 +963,14 @@ def get_positions_of_all_joints(self) -> Dict[str, float]: def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: """ - Updates the transforms of all links of this object using time 'transform_time' or the current ros time. + Update the transforms of all links of this object using time 'transform_time' or the current ros time. """ for link in self.links.values(): link.update_transform(transform_time) def contact_points(self) -> ContactPointsList: """ - Returns a list of contact points of this Object with other Objects. + Return a list of contact points of this Object with other Objects. :return: A list of all contact points with other objects """ @@ -975,7 +978,7 @@ def contact_points(self) -> ContactPointsList: def contact_points_simulated(self) -> ContactPointsList: """ - Returns a list of all contact points between this Object and other Objects after stepping the simulation once. + Return a list of all contact points between this Object and other Objects after stepping the simulation once. :return: A list of contact points between this Object and other Objects """ @@ -987,7 +990,7 @@ def contact_points_simulated(self) -> ContactPointsList: def closest_points(self, max_distance: float) -> ClosestPointsList: """ - Returns a list of closest points between this Object and other Objects. + Return a list of closest points between this Object and other Objects. :param max_distance: The maximum distance between the closest points :return: A list of closest points between this Object and other Objects @@ -996,7 +999,7 @@ def closest_points(self, max_distance: float) -> ClosestPointsList: def closest_points_with_obj(self, other_object: Object, max_distance: float) -> ClosestPointsList: """ - Returns a list of closest points between this Object and another Object. + Return a list of closest points between this Object and another Object. :param other_object: The other object :param max_distance: The maximum distance between the closest points @@ -1006,7 +1009,7 @@ def closest_points_with_obj(self, other_object: Object, max_distance: float) -> def set_color(self, rgba_color: Color) -> None: """ - Changes the color of this object, the color has to be given as a list + Change the color of this object, the color has to be given as a list of RGBA values. :param rgba_color: The color as Color object with RGBA values between 0 and 1 @@ -1021,7 +1024,7 @@ def set_color(self, rgba_color: Color) -> None: def get_color(self) -> Union[Color, Dict[str, Color]]: """ - This method returns the rgba_color of this object. The return is either: + Return the rgba_color of this object. The return is either: 1. A Color object with RGBA values, this is the case if the object only has one link (this happens for example if the object is spawned from a .obj or .stl file) @@ -1029,7 +1032,8 @@ def get_color(self) -> Union[Color, Dict[str, Color]]: Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which the object is spawned. - :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and the rgba_color as value. + :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and + the rgba_color as value. """ link_to_color_dict = self.links_colors @@ -1047,12 +1051,14 @@ def links_colors(self) -> Dict[str, Color]: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ + Return the axis aligned bounding box of this object. :return: The axis aligned bounding box of this object. """ return self.world.get_object_axis_aligned_bounding_box(self) def get_base_origin(self) -> Pose: """ + Return the origin of the base/bottom of this object. :return: the origin of the base/bottom of this object. """ aabb = self.get_axis_aligned_bounding_box() @@ -1063,7 +1069,7 @@ def get_base_origin(self) -> Pose: def get_joint_by_id(self, joint_id: int) -> Joint: """ - Returns the joint object with the given id. + Return the joint object with the given id. :param joint_id: The unique id of the joint. :return: The joint object. @@ -1072,7 +1078,7 @@ def get_joint_by_id(self, joint_id: int) -> Joint: def copy_to_prospection(self) -> Object: """ - Copies this object to the prospection world. + Copy this object to the prospection world. :return: The copied object in the prospection world. """ @@ -1083,7 +1089,7 @@ def copy_to_prospection(self) -> Object: def __copy__(self) -> Object: """ - Returns a copy of this object. The copy will have the same name, type, path, description, pose, world and color. + Return a copy of this object. The copy will have the same name, type, path, description, pose, world and color. :return: A copy of this object. """ @@ -1093,10 +1099,7 @@ def __copy__(self) -> Object: return obj def __eq__(self, other): - if not isinstance(other, Object): - return False - return (self.id == other.id and self.world == other.world and self.name == other.name - and self.obj_type == other.obj_type) + return (self.id == other.id and self.world == other.world) if isinstance(other, Object) else False def __hash__(self): - return hash((self.name, self.obj_type, self.id, self.world.id)) + return hash((self.id, self.world)) From 46061f46de61c08f99d5e15012b7c604bfeff48c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 15:43:18 +0200 Subject: [PATCH 097/189] [Multiverse] No need for SocketAddress class. --- src/pycram/config/multiverse_conf.py | 5 +++ .../multiverse_communication/clients.py | 4 +-- .../worlds/multiverse_communication/socket.py | 33 ++++++++----------- test/test_multiverse.py | 1 - 4 files changed, 20 insertions(+), 23 deletions(-) create mode 100644 src/pycram/config/multiverse_conf.py diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py new file mode 100644 index 000000000..b89acb263 --- /dev/null +++ b/src/pycram/config/multiverse_conf.py @@ -0,0 +1,5 @@ + + +HOST: str = "tcp://127.0.0.1" +SERVER_HOST: str = HOST +SERVER_PORT: str = "7000" diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index e3d5b47fa..7bd84fdaf 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -5,7 +5,7 @@ from typing_extensions import List, Dict, Tuple, Optional -from .socket import MultiverseSocket, MultiverseMetaData, SocketAddress +from .socket import MultiverseSocket, MultiverseMetaData from ..multiverse_datastructures.enums import MultiverseAPIName as API from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.pose import Pose @@ -26,7 +26,7 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name - super().__init__(SocketAddress(port=str(port)), meta_data) + super().__init__(port=str(port), meta_data=meta_data) self.run() diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 7353e95d6..5116cf27a 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -5,7 +5,7 @@ import logging from multiverse_client_pybind import MultiverseClientPybind # noqa -from typing_extensions import Optional +from ...config import multiverse_conf as conf T = TypeVar("T") @@ -21,30 +21,23 @@ class MultiverseMetaData: handedness: str = "rhs" -class SocketAddress: - host: str = "tcp://127.0.0.1" - port: str = "" - - def __init__(self, port: str) -> None: - self.port = port - - class MultiverseSocket: - _server_addr: SocketAddress = SocketAddress(port="7000") def __init__( self, - client_addr: SocketAddress, - multiverse_meta_data: MultiverseMetaData = MultiverseMetaData(), + port: str, + host: str = conf.HOST, + meta_data: MultiverseMetaData = MultiverseMetaData(), ) -> None: - if not isinstance(client_addr.port, str) or client_addr.port == "": + if not isinstance(port, str) or port == "": raise ValueError(f"Must specify client port for {self.__class__.__name__}") self._send_data = None - self._client_addr = client_addr - self._meta_data = multiverse_meta_data + self.port = port + self.host = host + self._meta_data = meta_data self.client_name = self._meta_data.simulation_name self._multiverse_socket = MultiverseClientPybind( - f"{self._server_addr.host}:{self._server_addr.port}" + f"{conf.SERVER_HOST}:{conf.SERVER_PORT}" ) self.request_meta_data = { "meta_data": self._meta_data.__dict__, @@ -53,7 +46,7 @@ def __init__( } def run(self) -> None: - message = f"[Client {self._client_addr.port}] Start {self.__class__.__name__}{self._client_addr.port}" + message = f"[Client {self.port}] Start {self.__class__.__name__}{self.port}" logging.info(message) self._connect_and_start() @@ -79,7 +72,7 @@ def request_meta_data(self, request_meta_data: Dict) -> None: def response_meta_data(self) -> Dict: response_meta_data = self._multiverse_socket.get_response_meta_data() if not response_meta_data: - message = f"[Client {self._client_addr.port}] Receive empty response meta data." + message = f"[Client {self.port}] Receive empty response meta data." logging.warning(message) return response_meta_data @@ -96,12 +89,12 @@ def send_data(self, send_data: List[float]) -> None: def receive_data(self) -> List[float]: receive_data = self._multiverse_socket.get_receive_data() if not receive_data: - message = f"[Client {self._client_addr.port}] Receive empty data." + message = f"[Client {self.port}] Receive empty data." logging.warning(message) return receive_data def _connect_and_start(self) -> None: - self._multiverse_socket.connect(self._client_addr.host, self._client_addr.port) + self._multiverse_socket.connect(self.host, self.port) self._multiverse_socket.start() def _disconnect(self) -> None: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9c4e80b37..94c97cbda 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -14,7 +14,6 @@ multiverse_installed = True try: from pycram.worlds.multiverse import Multiverse - from pycram.worlds.multiverse_communication.socket import SocketAddress except ImportError: multiverse_installed = False From 8310f020cfb1ad949fb3feac6c263e5ff3dc1bbe Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 15:56:46 +0200 Subject: [PATCH 098/189] [Multiverse] docstring --- .../worlds/multiverse_communication/client_manager.py | 6 ++++-- src/pycram/worlds/multiverse_communication/socket.py | 6 ++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 42f8cffa2..0dc0411d9 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -18,7 +18,6 @@ class MultiverseClientManager: def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': """ Create a Multiverse reader client. - param max_wait_time_for_data: The maximum wait time for the data in seconds. param is_prospection_world: Whether the reader is connected to the prospection world. """ return self.create_client(MultiverseReader, "reader", is_prospection_world) @@ -50,7 +49,10 @@ def create_client(self, MultiverseReader, MultiverseWriter]: """ Create a Multiverse client. - param kwargs: The keyword arguments. + param client_type: The type of the client to create. + param name: The name of the client. + param is_prospection_world: Whether the client is connected to the prospection world. + param kwargs: Any other keyword arguments that should be passed to the client constructor. """ MultiverseClientManager.last_used_port += 1 name = (name or client_type.__name__) + f"_{self.last_used_port}" diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 5116cf27a..d7f8fba83 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -29,6 +29,12 @@ def __init__( host: str = conf.HOST, meta_data: MultiverseMetaData = MultiverseMetaData(), ) -> None: + """ + Initialize the MultiverseSocket, connect to the Multiverse Server and start the communication. + :param port: The port of the client. + :param host: The host of the client. + :param meta_data: The metadata for the Multiverse Client as MultiverseMetaData. + """ if not isinstance(port, str) or port == "": raise ValueError(f"Must specify client port for {self.__class__.__name__}") self._send_data = None From e59175ca61bea93e48e7b7283d55fb1c0a9a5ba9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 16:26:33 +0200 Subject: [PATCH 099/189] [PyCRAMInit] import robot_descriptions --- src/pycram/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index 477a6d659..144050f82 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -1,4 +1,5 @@ import pycram.process_modules +import pycram.robot_descriptions # from .specialized_designators import * __version__ = "0.0.2" From 53237ac5ed442d4b994ede55b18ca69c18faf405 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 17:38:24 +0200 Subject: [PATCH 100/189] [MultiverseGoalValidation] Made the goal validator available globally not just for multiverse.py Added the use of validators to pybullet. --- src/pycram/datastructures/world.py | 28 ++++++++++++------- src/pycram/validation/__init__.py | 0 .../error_checkers.py | 0 .../goal_validator.py | 16 +++++------ src/pycram/world_concepts/world_object.py | 2 ++ src/pycram/worlds/bullet_world.py | 4 +++ src/pycram/worlds/multiverse.py | 2 +- test/test_error_checkers.py | 2 +- test/test_goal_validator.py | 4 +-- test/test_multiverse.py | 3 +- 10 files changed, 37 insertions(+), 24 deletions(-) create mode 100644 src/pycram/validation/__init__.py rename src/pycram/{worlds/multiverse_functions => validation}/error_checkers.py (100%) rename src/pycram/{worlds/multiverse_functions => validation}/goal_validator.py (97%) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b6ac4a53c..1a4e9a540 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -27,9 +27,11 @@ from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event from ..config import world_conf as conf -from ..worlds.multiverse_functions.goal_validator import (MultiPoseGoalValidator, - PoseGoalValidator, JointPositionGoalValidator, - MultiJointPositionGoalValidator, GoalValidator) +from pycram.validation.goal_validator import (MultiPoseGoalValidator, + PoseGoalValidator, JointPositionGoalValidator, + MultiJointPositionGoalValidator, GoalValidator, + validate_joint_position, validate_multiple_joint_positions, + validate_object_pose) if TYPE_CHECKING: from ..world_concepts.world_object import Object @@ -354,10 +356,8 @@ def get_object_by_name(self, name: str) -> Optional[Object]: :return: The object with the given name, if there is one. """ - object = list(filter(lambda obj: obj.name == name, self.objects)) - if len(object) > 0: - return object[0] - return None + matching_objects = list(filter(lambda obj: obj.name == name, self.objects)) + return matching_objects[0] if len(matching_objects) > 0 else None def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ @@ -726,21 +726,26 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, """ raise NotImplementedError + @validate_joint_position @abstractmethod def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation - + NOTE: It is recommended to use the validate_joint_position decorator to validate the joint position for + the implementation of this method. :param joint: The joint to reset the position for. :param joint_position: The new joint pose. :return: True if the reset was successful, False otherwise """ pass + @validate_multiple_joint_positions @abstractmethod def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. + NOTE: It is recommended to use the validate_multiple_joint_positions decorator to validate the + joint positions for the implementation of this method. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. :return: True if the set was successful, False otherwise. """ @@ -753,12 +758,14 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ pass + @validate_object_pose @abstractmethod def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. - + NOTE: It is recommended to use the validate_object_pose decorator to validate the object pose for the + implementation of this method. :param obj: The object. :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. @@ -1365,7 +1372,8 @@ def __init__(self): self.prev_world: Optional[World] = None # The previous world is saved to restore it after the with block is exited. - def sync_worlds(self): + @staticmethod + def sync_worlds(): """ Synchronizes the state of the prospection world with the main world. """ diff --git a/src/pycram/validation/__init__.py b/src/pycram/validation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/worlds/multiverse_functions/error_checkers.py b/src/pycram/validation/error_checkers.py similarity index 100% rename from src/pycram/worlds/multiverse_functions/error_checkers.py rename to src/pycram/validation/error_checkers.py diff --git a/src/pycram/worlds/multiverse_functions/goal_validator.py b/src/pycram/validation/goal_validator.py similarity index 97% rename from src/pycram/worlds/multiverse_functions/goal_validator.py rename to src/pycram/validation/goal_validator.py index 980faae9b..437ea1a50 100644 --- a/src/pycram/worlds/multiverse_functions/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -5,7 +5,7 @@ from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ +from pycram.validation.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker if TYPE_CHECKING: @@ -368,10 +368,10 @@ class JointPositionGoalValidator(GoalValidator): def __init__(self, current_position_getter: OptionalArgCallable = None, acceptable_error: Optional[float] = None, - acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, - acceptable_position_error: Optional[Iterable[float]] = 1e-3, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, - is_iterable: Optional[bool] = False): + acceptable_orientation_error: float = np.pi / 180, + acceptable_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8, + is_iterable: bool = False): """ Initialize the joint position goal validator. :param current_position_getter: The current position getter function which takes an optional input and returns @@ -412,9 +412,9 @@ class MultiJointPositionGoalValidator(GoalValidator): def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_error: Optional[Iterable[float]] = None, - acceptable_orientation_error: Optional[Iterable[float]] = np.pi / 180, - acceptable_position_error: Optional[Iterable[float]] = 1e-3, - acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): + acceptable_orientation_error: float = np.pi / 180, + acceptable_position_error: float = 1e-3, + acceptable_percentage_of_goal_achieved: float = 0.8): """ Initialize the multi-joint position goal validator. :param current_positions_getter: The current positions getter function which takes an optional input and diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d35e4242a..2b29b96a9 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -903,6 +903,7 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) + def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ Set the position of the given joint to the given joint pose and updates the poses of all attached objects. @@ -912,6 +913,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 171a34445..7059b2850 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -17,6 +17,7 @@ from ..datastructures.pose import Pose from ..object_descriptors.urdf import ObjectDescription from ..datastructures.world import World +from ..validation.goal_validator import validate_multiple_joint_positions, validate_joint_position, validate_object_pose from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object @@ -182,11 +183,13 @@ def parse_points_list_to_args(self, point: List) -> Dict: "lateral_friction_1": LateralFriction(point[10], point[11]), "lateral_friction_2": LateralFriction(point[12], point[13])} + @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: for joint, joint_position in joint_positions.items(): self.reset_joint_position(joint, joint_position) return True + @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: p.resetJointState(joint.object_id, joint.id, joint_position, physicsClientId=self.id) return True @@ -198,6 +201,7 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) + @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index e663684a3..f89427293 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -7,7 +7,7 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from .multiverse_functions.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ +from pycram.validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType diff --git a/test/test_error_checkers.py b/test/test_error_checkers.py index b7a459ff9..63bf06416 100644 --- a/test/test_error_checkers.py +++ b/test/test_error_checkers.py @@ -4,7 +4,7 @@ from tf.transformations import quaternion_from_euler from pycram.datastructures.enums import JointType -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions, \ +from pycram.validation.error_checkers import calculate_angle_between_quaternions, \ PoseErrorChecker, PositionErrorChecker, OrientationErrorChecker, RevoluteJointPositionErrorChecker, \ PrismaticJointPositionErrorChecker, MultiJointPositionErrorChecker diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 6ebfe04ff..9b79cc114 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -6,10 +6,10 @@ from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription -from pycram.worlds.multiverse_functions.error_checkers import PoseErrorChecker, PositionErrorChecker, \ +from pycram.validation.error_checkers import PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, RevoluteJointPositionErrorChecker, PrismaticJointPositionErrorChecker, \ MultiJointPositionErrorChecker -from pycram.worlds.multiverse_functions.goal_validator import GoalValidator, PoseGoalValidator, \ +from pycram.validation.goal_validator import GoalValidator, PoseGoalValidator, \ PositionGoalValidator, OrientationGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, \ MultiPoseGoalValidator, MultiPositionGoalValidator, MultiOrientationGoalValidator diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 9ea99398a..f01483a0a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import time import unittest import numpy as np @@ -13,7 +12,7 @@ from pycram.designators.object_designator import BelieveObject from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object -from pycram.worlds.multiverse_functions.error_checkers import calculate_angle_between_quaternions +from pycram.validation.error_checkers import calculate_angle_between_quaternions multiverse_installed = True try: From 31962cb8454cd268cda3647b2e298779060964f9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 19:54:38 +0200 Subject: [PATCH 101/189] [MultiverseController] Fix issues after Merge with MultiverseGoalValidation --- .../worlds/multiverse_communication/client_manager.py | 4 ++-- src/pycram/worlds/multiverse_datastructures/enums.py | 2 +- test/test_multiverse.py | 9 +++++---- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 324a193d2..9eb0b8f75 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -1,6 +1,6 @@ -from typing_extensions import Optional, Type, Union, List, Dict +from typing_extensions import Optional, Type, Union, Dict -from pycram.worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ +from ...worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ MultiverseReader, MultiverseController diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index cf87410ed..152c80280 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -1,7 +1,7 @@ from enum import Enum from pycram.datastructures.enums import JointType -from ..multiverse_functions.exceptions import UnsupportedJointType +from ..multiverse_extras.exceptions import UnsupportedJointType class MultiverseAPIName(Enum): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4e91ba0a0..254fd9cd2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 +import os import unittest -from time import sleep import numpy as np import psutil @@ -18,6 +18,7 @@ from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.worlds.multiverse_extras.helpers import get_robot_mjcf_path, parse_mjcf_actuators multiverse_installed = True try: @@ -360,11 +361,11 @@ def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, position_delta = self.multiverse.acceptable_position_error if orientation_delta is None: orientation_delta = self.multiverse.acceptable_orientation_error - self.assert_positon_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) + self.assert_position_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) - def assert_positon_is_equal(self, position1: List[float], position2: List[float], delta: Optional[float] = None): + def assert_position_is_equal(self, position1: List[float], position2: List[float], delta: Optional[float] = None): if delta is None: delta = self.multiverse.acceptable_position_error self.assert_list_is_equal(position1, position2, delta=delta) @@ -377,4 +378,4 @@ def assert_orientation_is_equal(self, orientation1: List[float], orientation2: L def assert_list_is_equal(self, list1: List, list2: List, delta: float): for i in range(len(list1)): - self.assertAlmostEqual(list1[i], list2[i], delta=delta) \ No newline at end of file + self.assertAlmostEqual(list1[i], list2[i], delta=delta) From d5e36cd5c8cb58bc96edd726a7bd85e6e663dfc5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 20:02:11 +0200 Subject: [PATCH 102/189] [MultiverseController] corrected datatype from float to datetime. increased wait time for removing the robot. --- src/pycram/worlds/multiverse.py | 5 +++-- test/test_multiverse.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index af28224bb..6508c0c55 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,3 +1,4 @@ +import datetime import logging import os from time import sleep @@ -68,7 +69,7 @@ class Multiverse(World): Whether to use the joint controller or not. """ - REMOVE_ROBOT_WAIT_TIME: float = 0.5 + REMOVE_ROBOT_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=700) """ The time to wait after removing the robot from the simulator. """ @@ -422,7 +423,7 @@ def remove_object_from_simulator(self, obj: Object) -> None: if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) if obj.obj_type == ObjectType.ROBOT: - sleep(self.REMOVE_ROBOT_WAIT_TIME) + sleep(self.REMOVE_ROBOT_WAIT_TIME.total_seconds()) def add_constraint(self, constraint: Constraint) -> int: diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 254fd9cd2..5958b0c4e 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -50,7 +50,7 @@ def setUpClass(cls): return cls.multiverse = Multiverse(simulation="pycram_test", is_prospection=False, - use_controller=False) + use_controller=True) @classmethod def tearDownClass(cls): From ec089023e56ba495e1f35f6a7c58a1dd266cb57b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 31 Jul 2024 20:14:28 +0200 Subject: [PATCH 103/189] [MultiverseController] add sleep after remove in test --- test/test_bullet_world.py | 1 + 1 file changed, 1 insertion(+) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 05cf36cff..46979293b 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -148,6 +148,7 @@ def test_no_prospection_object_found_for_given_object(self): try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) + time.sleep(0.1) self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) except KeyError as e: From 8aaae44d62ea09116ee78cb44efa8af2f85ae05f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 1 Aug 2024 18:53:25 +0200 Subject: [PATCH 104/189] [MultiverseController] made use of the config files. Added __eq__ for JointState, LinkState, ObjectState, and WorldState. Added acceptable error in ObjectState, and JointState, for Pose, and joint_position. Added almost_equal method in pose.py --- src/pycram/config/multiverse_conf.py | 13 +++- src/pycram/config/world_conf.py | 57 ++++++++++++--- src/pycram/datastructures/dataclasses.py | 57 ++++++++++++++- src/pycram/datastructures/pose.py | 36 ++++------ src/pycram/datastructures/world.py | 69 ++++++++++++------- src/pycram/description.py | 9 ++- src/pycram/validation/error_checkers.py | 7 +- src/pycram/validation/goal_validator.py | 24 +++---- src/pycram/world_concepts/world_object.py | 11 ++- src/pycram/worlds/multiverse.py | 55 +++++++-------- .../multiverse_communication/clients.py | 2 +- test/test_bullet_world.py | 1 - 12 files changed, 225 insertions(+), 116 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index b89acb263..791e1713d 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -1,5 +1,16 @@ - +from . import world_conf as conf HOST: str = "tcp://127.0.0.1" SERVER_HOST: str = HOST SERVER_PORT: str = "7000" + +simulation_time_step: float = 1e-3 +simulation_frequency: int = int(1 / simulation_time_step) + +use_controller: bool = False + +job_handling: conf.JobHandling = conf.JobHandling(let_pycram_move_attached_objects=False, + let_pycram_handle_spawning=False) + +error_tolerance: conf.ErrorTolerance = conf.ErrorTolerance(acceptable_position_error=2e-2, + acceptable_prismatic_joint_position_error=2e-2) diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index a8f54647c..1e2e9181a 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -1,6 +1,7 @@ +import math import os +from dataclasses import dataclass -import numpy as np from typing_extensions import Tuple resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') @@ -19,16 +20,50 @@ The prefix for the prospection world name. """ -acceptable_position_error: float = 5e-3 -acceptable_orientation_error: float = 10 * np.pi / 180 -acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) -use_percentage_of_goal: bool = True -acceptable_percentage_of_goal: float = 0.5 +update_poses_from_sim_on_get: bool = True """ -The acceptable error for the position and orientation of an object/link. +Whether to update the poses from the simulator when getting the object poses. """ -raise_goal_validator_error: bool = False -""" -Whether to raise an error if the goals are not achieved. -""" \ No newline at end of file + +@dataclass +class JobHandling: + let_pycram_move_attached_objects: bool = True + let_pycram_handle_spawning: bool = True + let_pycram_handle_world_sync: bool = True + """ + Whether to let PyCRAM handle the movement of attached objects, the spawning of objects, + and the world synchronization. + """ + + def as_dict(self): + return self.__dict__ + + +@dataclass +class ErrorTolerance: + acceptable_position_error: float = 1e-2 + acceptable_orientation_error: float = 10 * math.pi / 180 + acceptable_prismatic_joint_position_error: float = 1e-2 + acceptable_revolute_joint_position_error: float = 5 * math.pi / 180 + """ + The acceptable error for the position and orientation of an object/link, and the joint positions. + """ + + use_percentage_of_goal: bool = False + acceptable_percentage_of_goal: float = 0.5 + """ + Whether to use a percentage of the goal as the acceptable error. + """ + + raise_goal_validator_error: bool = False + """ + Whether to raise an error if the goals are not achieved. + """ + + @property + def acceptable_pose_error(self) -> Tuple[float, float]: + return self.acceptable_position_error, self.acceptable_orientation_error + + def as_dict(self): + return self.__dict__ diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index ebcb9d346..5f393475f 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -1,10 +1,14 @@ from __future__ import annotations +from abc import ABC, abstractmethod from dataclasses import dataclass + from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING + from .enums import JointType, Shape, VirtualMoveBaseJointName from .pose import Pose, Point -from abc import ABC, abstractmethod +from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable +from ..config import world_conf as conf if TYPE_CHECKING: from ..description import Link @@ -284,6 +288,17 @@ class LinkState(State): """ constraint_ids: Dict[Link, int] + def __eq__(self, other: 'LinkState'): + return self.all_constraints_exist(other) and self.all_constraints_are_equal(other) + + def all_constraints_exist(self, other: 'LinkState'): + return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) + and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) + + def all_constraints_are_equal(self, other: 'LinkState'): + return all([cid == other_cid for cid, other_cid in zip(self.constraint_ids.values(), + other.constraint_ids.values())]) + @dataclass class JointState(State): @@ -291,6 +306,11 @@ class JointState(State): Dataclass for storing the state of a joint. """ position: float + acceptable_error: float + + def __eq__(self, other: 'JointState'): + error = calculate_joint_position_error(self.position, other.position) + return is_error_acceptable(error, other.acceptable_error) @dataclass @@ -302,6 +322,23 @@ class ObjectState(State): attachments: Dict[Object, Attachment] link_states: Dict[int, LinkState] joint_states: Dict[int, JointState] + acceptable_pose_error: Tuple[float, float] + + def __eq__(self, other: 'ObjectState'): + return (self.pose_is_almost_equal(other) + and self.all_attachments_exist(other) and self.all_attachments_are_equal(other) + and self.link_states == other.link_states + and self.joint_states == other.joint_states) + + def pose_is_almost_equal(self, other: 'ObjectState'): + return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) + + def all_attachments_exist(self, other: 'ObjectState'): + return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) + and len(self.attachments.keys()) == len(other.attachments.keys())) + + def all_attachments_are_equal(self, other: 'ObjectState'): + return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) @dataclass @@ -312,6 +349,22 @@ class WorldState(State): simulator_state_id: int object_states: Dict[str, ObjectState] + 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)) + + def simulator_state_is_equal(self, other: 'WorldState'): + return self.simulator_state_id == other.simulator_state_id + + def all_objects_exist(self, other: 'WorldState'): + return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) + and len(self.object_states.keys()) == len(other.object_states.keys())) + + def all_objects_states_are_equal(self, other: 'WorldState'): + return all([obj_state == other_obj_state + for obj_state, other_obj_state in zip(self.object_states.values(), + other.object_states.values())]) + @dataclass class LateralFriction: @@ -486,5 +539,3 @@ def get_axes(self) -> Dict[str, Point]: return {self.translation_x: Point(1, 0, 0), self.translation_y: Point(0, 1, 0), self.angular_z: Point(0, 0, 1)} - - diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 6df570720..4a7796453 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -12,6 +12,7 @@ from geometry_msgs.msg import (Pose as GeoPose, Quaternion as GeoQuaternion) from tf import transformations from ..orm.base import Pose as ORMPose, Position, Quaternion, ProcessMetaData +from ..validation.error_checkers import calculate_pose_error def get_normalized_quaternion(quaternion: np.ndarray) -> GeoQuaternion: @@ -233,6 +234,20 @@ def __eq__(self, other: Pose) -> bool: return self_position == other_position and self_orient == other_orient and self.frame == other.frame + def almost_equal(self, other: Pose, position_tolerance_in_meters: float = 1e-3, + orientation_tolerance_in_degrees: float = 1) -> bool: + """ + Checks if the given Pose is almost equal to this Pose. The position and orientation can have a certain + tolerance. The position tolerance is given in meters and the orientation tolerance in degrees. The position + error is calculated as the euclidian distance between the positions and the orientation error as the angle + between the quaternions. + :param other: The other Pose which should be compared + :param position_tolerance_in_meters: The tolerance for the position in meters + :param orientation_tolerance_in_degrees: The tolerance for the orientation in degrees + """ + error = calculate_pose_error(self, other) + return error[0] <= position_tolerance_in_meters and error[1] <= orientation_tolerance_in_degrees * math.pi / 180 + def set_position(self, new_position: List[float]) -> None: """ Sets the position of this Pose to the given position. Position has to be given as a vector in cartesian space. @@ -288,25 +303,6 @@ def multiply_quaternions(self, quaternion: List) -> None: self.orientation = (x, y, z, w) - def set_orientation_from_euler(self, axis: List, euler_angles: List[float]) -> None: - """ - Convert axis-angle to quaternion. - - :param axis: (x, y, z) tuple representing rotation axis. - :param angle: rotation angle in degree - :return: The quaternion representing the axis angle - """ - angle = math.radians(euler_angles) - axis_length = math.sqrt(sum([i ** 2 for i in axis])) - normalized_axis = tuple(i / axis_length for i in axis) - - x = normalized_axis[0] * math.sin(angle / 2) - y = normalized_axis[1] * math.sin(angle / 2) - z = normalized_axis[2] * math.sin(angle / 2) - w = math.cos(angle / 2) - - return (x, y, z, w) - class Transform(TransformStamped): """ @@ -556,5 +552,3 @@ def set_rotation(self, new_rotation: List[float]) -> None: :param new_rotation: The new rotation as a quaternion with xyzw """ self.rotation = new_rotation - - diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 3d3efe155..a12fb1f97 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -92,41 +92,60 @@ class World(StateEntity, ABC): The prefix for the prospection world name. """ - acceptable_position_error: float = 5e-2 # 5 cm - acceptable_orientation_error: float = 10 * np.pi / 180 # 10 degrees - acceptable_pose_error: Tuple[float, float] = (acceptable_position_error, acceptable_orientation_error) - use_percentage_of_goal: bool = False - acceptable_percentage_of_goal: float = 0.5 if use_percentage_of_goal else None # 50% + let_pycram_move_attached_objects: bool = conf.JobHandling.let_pycram_move_attached_objects + let_pycram_handle_spawning: bool = conf.JobHandling.let_pycram_handle_spawning + let_pycram_handle_world_sync: bool = conf.JobHandling.let_pycram_handle_world_sync + """ + Whether to let PyCRAM handle the movement of attached objects, the spawning of objects, + and the world synchronization. + """ + + update_poses_from_sim_on_get: bool = conf.update_poses_from_sim_on_get + """ + Whether to update the poses from the simulator when getting the object poses. + """ + + acceptable_position_error: float = conf.ErrorTolerance.acceptable_position_error + acceptable_orientation_error: float = conf.ErrorTolerance.acceptable_orientation_error + acceptable_pose_error: Tuple[float, float] = conf.ErrorTolerance.acceptable_pose_error + acceptable_revolute_joint_position_error: float = conf.ErrorTolerance.acceptable_revolute_joint_position_error + acceptable_prismatic_joint_position_error: float = conf.ErrorTolerance.acceptable_prismatic_joint_position_error + use_percentage_of_goal: bool = conf.ErrorTolerance.use_percentage_of_goal + acceptable_percentage_of_goal: float = conf.ErrorTolerance.acceptable_percentage_of_goal """ The acceptable error for the position and orientation of an object/link. """ - raise_goal_validator_error: Optional[bool] = False + raise_goal_validator_error: Optional[bool] = conf.ErrorTolerance.raise_goal_validator_error """ Whether to raise an error if the goals are not achieved. """ - def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float): + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, + **config_kwargs): """ - Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the - background. There can only be one rendered simulation. - The World object also initializes the Events for attachment, detachment and for manipulating the world. + Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the + background. There can only be one rendered simulation. + The World object also initializes the Events for attachment, detachment and for manipulating the world. - :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" - :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is + "GUI" + :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. + :param simulation_frequency: The frequency of the simulation in Hz. + :param config_kwargs: Additional configuration parameters. """ StateEntity.__init__(self) - self.let_pycram_move_attached_objects = True - self.let_pycram_handle_spawning = True - self.let_pycram_handle_world_sync = True - self.update_poses_from_sim_on_get = True + # Parse config_kwargs + for key, value in config_kwargs.items(): + setattr(self, key, value) + GoalValidator.raise_error = World.raise_goal_validator_error + World.simulation_frequency = simulation_frequency if World.current_world is None: World.current_world = self - World.simulation_frequency = simulation_frequency self.object_lock: threading.Lock = threading.Lock() @@ -203,13 +222,13 @@ def _init_goal_validators(self): # Joint Goal validators self.joint_position_goal_validator = JointPositionGoalValidator( self.get_joint_position, - acceptable_orientation_error=self.acceptable_orientation_error, - acceptable_position_error=self.acceptable_position_error, + acceptable_revolute_joint_position_error=self.acceptable_revolute_joint_position_error, + acceptable_prismatic_joint_position_error=self.acceptable_prismatic_joint_position_error, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), - acceptable_orientation_error=self.acceptable_orientation_error, - acceptable_position_error=self.acceptable_position_error, + acceptable_revolute_joint_position_error=self.acceptable_revolute_joint_position_error, + acceptable_prismatic_joint_position_error=self.acceptable_prismatic_joint_position_error, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) def check_object_exists(self, obj: Object): @@ -941,12 +960,14 @@ def save_state(self, state_id: Optional[int] = None) -> int: def current_state(self) -> WorldState: if self._current_state is None: self._current_state = WorldState(self.save_physics_simulator_state(), self.object_states) - return self._current_state + return WorldState(self._current_state.simulator_state_id, self.object_states) @current_state.setter def current_state(self, state: WorldState) -> None: - self.restore_physics_simulator_state(state.simulator_state_id) - self.object_states = state.object_states + if self.current_state != state: + self.restore_physics_simulator_state(state.simulator_state_id) + for obj in self.objects: + self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] @property def object_states(self) -> Dict[str, ObjectState]: diff --git a/src/pycram/description.py b/src/pycram/description.py index 1c199ff8e..43be29359 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -262,7 +262,8 @@ def current_state(self) -> LinkState: @current_state.setter def current_state(self, link_state: LinkState) -> None: - self.constraint_ids = link_state.constraint_ids + if self.current_state != link_state: + self.constraint_ids = link_state.constraint_ids def add_fixed_constraint_with_link(self, child_link: 'Link', child_to_parent_transform: Optional[Transform] = None) -> int: @@ -495,6 +496,8 @@ def __init__(self, _id: int, obj: Object, is_virtual: Optional[bool] = False): ObjectEntity.__init__(self, _id, obj) JointDescription.__init__(self, joint_description.parsed_description, is_virtual) + self.acceptable_error = (self.world.acceptable_revolute_joint_position_error if self.type == JointType.REVOLUTE + else self.world.acceptable_prismatic_joint_position_error) self._update_position() @property @@ -589,7 +592,7 @@ def get_applied_motor_torque(self) -> float: @property def current_state(self) -> JointState: - return JointState(self.position) + return JointState(self.position, self.acceptable_error) @current_state.setter def current_state(self, joint_state: JointState) -> None: @@ -598,7 +601,7 @@ def current_state(self, joint_state: JointState) -> None: :param joint_state: The joint state to update from. """ - if self._current_position != joint_state.position: + if self.current_state != joint_state: self.position = joint_state.position def __copy__(self): diff --git a/src/pycram/validation/error_checkers.py b/src/pycram/validation/error_checkers.py index 4087befd2..c78e18c7a 100644 --- a/src/pycram/validation/error_checkers.py +++ b/src/pycram/validation/error_checkers.py @@ -3,10 +3,11 @@ import numpy as np from tf.transformations import quaternion_multiply, quaternion_inverse -from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable +from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable, TYPE_CHECKING from pycram.datastructures.enums import JointType -from pycram.datastructures.pose import Pose +if TYPE_CHECKING: + from pycram.datastructures.pose import Pose class ErrorChecker(ABC): @@ -185,7 +186,7 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: return calculate_joint_position_error(value_1, value_2) -def calculate_pose_error(pose_1: Pose, pose_2: Pose) -> List[float]: +def calculate_pose_error(pose_1: 'Pose', pose_2: 'Pose') -> List[float]: """ Calculate the error between two poses. return: The error between the two poses. diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index cd608574c..9f2d4e8f0 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -368,8 +368,8 @@ class JointPositionGoalValidator(GoalValidator): def __init__(self, current_position_getter: OptionalArgCallable = None, acceptable_error: Optional[float] = None, - acceptable_orientation_error: float = np.pi / 180, - acceptable_position_error: float = 1e-3, + acceptable_revolute_joint_position_error: float = np.pi / 180, + acceptable_prismatic_joint_position_error: float = 1e-3, acceptable_percentage_of_goal_achieved: float = 0.8, is_iterable: bool = False): """ @@ -377,15 +377,15 @@ def __init__(self, current_position_getter: OptionalArgCallable = None, :param current_position_getter: The current position getter function which takes an optional input and returns the current position. :param acceptable_error: The acceptable error. - :param acceptable_orientation_error: The acceptable orientation error. - :param acceptable_position_error: The acceptable position error. + :param acceptable_revolute_joint_position_error: The acceptable orientation error. + :param acceptable_prismatic_joint_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. :param is_iterable: Whether it is a sequence of joint positions. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=is_iterable), current_position_getter, acceptable_percentage_of_goal_achieved=acceptable_percentage_of_goal_achieved) - self.acceptable_orientation_error = acceptable_orientation_error - self.acceptable_position_error = acceptable_position_error + self.acceptable_orientation_error = acceptable_revolute_joint_position_error + self.acceptable_position_error = acceptable_prismatic_joint_position_error def register_goal(self, goal_value: Any, joint_type: JointType, current_value_getter_input: Optional[Any] = None, @@ -412,22 +412,22 @@ class MultiJointPositionGoalValidator(GoalValidator): def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_error: Optional[Iterable[float]] = None, - acceptable_orientation_error: float = np.pi / 180, - acceptable_position_error: float = 1e-3, + acceptable_revolute_joint_position_error: float = np.pi / 180, + acceptable_prismatic_joint_position_error: float = 1e-3, acceptable_percentage_of_goal_achieved: float = 0.8): """ Initialize the multi-joint position goal validator. :param current_positions_getter: The current positions getter function which takes an optional input and returns the current positions. :param acceptable_error: The acceptable error. - :param acceptable_orientation_error: The acceptable orientation error. - :param acceptable_position_error: The acceptable position error. + :param acceptable_revolute_joint_position_error: The acceptable orientation error. + :param acceptable_prismatic_joint_position_error: The acceptable position error. :param acceptable_percentage_of_goal_achieved: The acceptable percentage of goal achieved. """ super().__init__(SingleValueErrorChecker(acceptable_error, is_iterable=True), current_positions_getter, acceptable_percentage_of_goal_achieved) - self.acceptable_orientation_error = acceptable_orientation_error - self.acceptable_position_error = acceptable_position_error + self.acceptable_orientation_error = acceptable_revolute_joint_position_error + self.acceptable_position_error = acceptable_prismatic_joint_position_error def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], current_value_getter_input: Optional[Any] = None, diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 5ef0ba2c6..e1c8ce0be 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -691,19 +691,18 @@ def current_state(self) -> ObjectState: The current state of this object as an ObjectState. """ return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), - self.joint_states.copy()) + self.joint_states.copy(), self.world.acceptable_pose_error) @current_state.setter def current_state(self, state: ObjectState) -> None: """ Set the current state of this object to the given state. """ - if self.get_pose().dist(state.pose) != 0.0: + if self.current_state != state: self.set_pose(state.pose, base=False, set_attachments=False) - - self.set_attachments(state.attachments) - self.link_states = state.link_states - self.joint_states = state.joint_states + self.set_attachments(state.attachments) + self.link_states = state.link_states + self.joint_states = state.joint_states def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6508c0c55..16dd5096c 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -10,8 +10,6 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty -from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ - validate_joint_position from .multiverse_extras.helpers import find_multiverse_resources_path from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -19,8 +17,11 @@ from ..datastructures.world import World from ..description import Link, Joint from ..robot_description import RobotDescription +from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ + validate_joint_position from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object +from ..config import multiverse_conf as conf class Multiverse(World): @@ -64,11 +65,6 @@ class Multiverse(World): is completed. """ - use_controller: Optional[bool] = None - """ - Whether to use the joint controller or not. - """ - REMOVE_ROBOT_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=700) """ The time to wait after removing the robot from the simulator. @@ -76,9 +72,9 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, - simulation_frequency: Optional[float] = 60.0, + simulation_frequency: float = conf.simulation_frequency, simulation: Optional[str] = None, - use_controller: Optional[bool] = False): + use_controller: bool = conf.use_controller): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -99,15 +95,15 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation - if Multiverse.use_controller is None: - Multiverse.use_controller = use_controller + # Whether to use the controller for the robot joints or not. + self.use_controller = use_controller - World.__init__(self, mode, is_prospection, simulation_frequency) + World.__init__(self, mode, is_prospection, simulation_frequency, **conf.job_handling.as_dict(), + **conf.error_tolerance.as_dict()) + self.client_manager = MultiverseClientManager(self.simulation_wait_time_factor) self._init_clients() - self._set_world_job_flags() - self._init_constraint_and_object_id_name_map_collections() if not self.is_prospection_world: @@ -116,20 +112,16 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.api_requester.pause_simulation() def _init_clients(self): - client_manager = MultiverseClientManager(self.simulation_wait_time_factor) - self.reader: MultiverseReader = client_manager.create_reader(is_prospection_world=self.is_prospection_world) - self.writer: MultiverseWriter = client_manager.create_writer(self.simulation, - is_prospection_world=self.is_prospection_world) - self.api_requester: MultiverseAPI = client_manager.create_api_requester( + self.reader: MultiverseReader = self.client_manager.create_reader( + is_prospection_world=self.is_prospection_world) + self.writer: MultiverseWriter = self.client_manager.create_writer( self.simulation, is_prospection_world=self.is_prospection_world) - self.joint_controller: MultiverseController = client_manager.create_controller( + self.api_requester: MultiverseAPI = self.client_manager.create_api_requester( + self.simulation, + is_prospection_world=self.is_prospection_world) + self.joint_controller: MultiverseController = self.client_manager.create_controller( is_prospection_world=self.is_prospection_world) - - def _set_world_job_flags(self): - self.let_pycram_move_attached_objects = False - self.let_pycram_handle_spawning = False - self.update_poses_from_sim_on_get = True def _init_constraint_and_object_id_name_map_collections(self): self.last_object_id: int = -1 @@ -206,7 +198,7 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: param pose: The pose of the object. return: The object id. """ - if object_type == ObjectType.ROBOT and Multiverse.use_controller: + if object_type == ObjectType.ROBOT and self.use_controller: self.spawn_robot_with_controller(name, pose) else: self._set_body_pose(name, pose) @@ -242,7 +234,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - if Multiverse.use_controller and self.joint_has_actuator(joint): + if self.use_controller and self.joint_has_actuator(joint): self._reset_joint_position_using_controller(joint, joint_position) else: self._set_multiple_joint_positions_without_controller({joint: joint_position}) @@ -259,7 +251,7 @@ def _reset_joint_position_using_controller(self, joint: Joint, joint_position: f @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: - if Multiverse.use_controller: + if self.use_controller: controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) if len(controlled_joints) > 0: controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints} @@ -282,7 +274,7 @@ def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -365,7 +357,8 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: """ self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: - self.xyzw_to_wxyz(pose.orientation_as_list())} + self.xyzw_to_wxyz(pose.orientation_as_list()), + MultiverseBodyProperty.RELATIVE_VELOCITY: [0, 0, 0, 0, 0, 0]} for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: @@ -420,6 +413,8 @@ def remove_object_by_id(self, obj_id: int) -> None: self.remove_object_from_simulator(obj) def remove_object_from_simulator(self, obj: Object) -> None: + if obj.obj_type == ObjectType.ROBOT: + sleep(self.REMOVE_ROBOT_WAIT_TIME.total_seconds()) if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) if obj.obj_type == ObjectType.ROBOT: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 5c954f7ee..912f8fae2 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -705,7 +705,7 @@ def _request_single_api_callback(self, api_name: API, *params) -> List[str]: param api_data: The API data to request the callback. return: The API response as a list of strings. """ - response = self._request_apis_callbacks({api_name: params}) + response = self._request_apis_callbacks({api_name: list(params)}) return response[api_name] def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[str]]: diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 46979293b..05cf36cff 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -148,7 +148,6 @@ def test_no_prospection_object_found_for_given_object(self): try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) - time.sleep(0.1) self.world.get_prospection_object_for_object(milk_2) self.assertFalse(True) except KeyError as e: From d16fa0451238af54ade72d95bff0a538f6d39902 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 1 Aug 2024 19:01:49 +0200 Subject: [PATCH 105/189] [MultiverseController] removed use of relative velocity and wait time for removing the robot. --- src/pycram/worlds/multiverse.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 16dd5096c..06d9e77fc 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -65,11 +65,6 @@ class Multiverse(World): is completed. """ - REMOVE_ROBOT_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=700) - """ - The time to wait after removing the robot from the simulator. - """ - def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, @@ -357,8 +352,7 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: """ self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: - self.xyzw_to_wxyz(pose.orientation_as_list()), - MultiverseBodyProperty.RELATIVE_VELOCITY: [0, 0, 0, 0, 0, 0]} + self.xyzw_to_wxyz(pose.orientation_as_list())} for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: @@ -413,12 +407,8 @@ def remove_object_by_id(self, obj_id: int) -> None: self.remove_object_from_simulator(obj) def remove_object_from_simulator(self, obj: Object) -> None: - if obj.obj_type == ObjectType.ROBOT: - sleep(self.REMOVE_ROBOT_WAIT_TIME.total_seconds()) if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) - if obj.obj_type == ObjectType.ROBOT: - sleep(self.REMOVE_ROBOT_WAIT_TIME.total_seconds()) def add_constraint(self, constraint: Constraint) -> int: From a2b95d50dc80d7690fa6e73884bb59cbdb7a8c43 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 2 Aug 2024 11:05:07 +0200 Subject: [PATCH 106/189] [MultiverseAction] Tests are working after merge with MultiverseController. Removed the use_controller argument, and made it global, and can only be set through multiverse_conf.py --- src/pycram/config/multiverse_conf.py | 2 +- src/pycram/worlds/multiverse.py | 26 +++++++++++++------------- test/test_multiverse.py | 14 +++++++------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 791e1713d..f21c4cf2f 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -4,7 +4,7 @@ SERVER_HOST: str = HOST SERVER_PORT: str = "7000" -simulation_time_step: float = 1e-3 +simulation_time_step: float = 1e-2 simulation_frequency: int = int(1 / simulation_time_step) use_controller: bool = False diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 9d148cdee..b05fe19ec 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -53,6 +53,11 @@ class Multiverse(World): the multiverse configuration file). """ + use_controller: bool = conf.use_controller + """ + Whether to use the controller for the robot joints or not. + """ + try: simulation_wait_time_factor = float(os.environ['Multiverse_Simulation_Wait_Time_Factor']) except KeyError: @@ -68,8 +73,7 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, - simulation: Optional[str] = None, - use_controller: bool = conf.use_controller): + simulation: Optional[str] = None): """ Initialize the Multiverse Socket and the PyCram World. param mode: The mode of the world (DIRECT or GUI). @@ -77,7 +81,6 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, param simulation_frequency: The frequency of the simulation. param client_addr: The address of the multiverse client. param simulation: The name of the simulation. - param use_controller: Whether to use the controller or not. """ self._make_sure_multiverse_resources_are_added() @@ -90,9 +93,6 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation - # Whether to use the controller for the robot joints or not. - self.use_controller = use_controller - World.__init__(self, mode, is_prospection, simulation_frequency, **conf.job_handling.as_dict(), **conf.error_tolerance.as_dict()) @@ -115,8 +115,9 @@ def _init_clients(self): self.api_requester: MultiverseAPI = self.client_manager.create_api_requester( self.simulation, is_prospection_world=self.is_prospection_world) - self.joint_controller: MultiverseController = self.client_manager.create_controller( - is_prospection_world=self.is_prospection_world) + if self.use_controller: + self.joint_controller: MultiverseController = self.client_manager.create_controller( + is_prospection_world=self.is_prospection_world) def _init_constraint_and_object_id_name_map_collections(self): self.last_object_id: int = -1 @@ -233,8 +234,6 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: self._reset_joint_position_using_controller(joint, joint_position) else: self._set_multiple_joint_positions_without_controller({joint: joint_position}) - # self.writer.set_body_property(joint.name, self.get_joint_position_name(joint.type), - # [joint_position]) return True def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: @@ -442,7 +441,7 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - self.step() + self.simulate(0.1) multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -518,7 +517,7 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L def step(self): self.api_requester.unpause_simulation() - sleep(30 / self.simulation_frequency) + sleep(self.simulation_time_step) self.api_requester.pause_simulation() def save_physics_simulator_state(self) -> int: @@ -552,7 +551,8 @@ def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingB raise NotImplementedError def set_realtime(self, real_time: bool) -> None: - logging.warning("set_realtime is not implemented in Multiverse") + 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") def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 8812bbef9..d7ba63b97 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -49,8 +49,7 @@ def setUpClass(cls): if not multiverse_installed: return cls.multiverse = Multiverse(simulation="pycram_test", - is_prospection=False, - use_controller=True) + is_prospection=False) @classmethod def tearDownClass(cls): @@ -102,7 +101,7 @@ def test_demo(self): milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) - TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() @staticmethod @with_simulated_robot @@ -130,10 +129,11 @@ def test_reset_world(self): self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_spawn_robot_with_actuators_directly_from_multiverse(self): - robot_name = "tiago_dual" - rdm = RobotDescriptionManager() - rdm.load_description(robot_name) - self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001])) + if self.multiverse.use_controller: + robot_name = "tiago_dual" + rdm = RobotDescriptionManager() + rdm.load_description(robot_name) + self.multiverse.spawn_robot_with_controller(robot_name, Pose([-2, -2, 0.001])) def test_spawn_object(self): milk = self.spawn_milk([1, 1, 0.1]) From b33a1ec19dde1167ee285069cf11dcaa28e0349b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 2 Aug 2024 11:50:29 +0200 Subject: [PATCH 107/189] [MultiverseAction] Introduced bullet_mode where the multiverse behaves exactly like bullet_world. The simulation is always in paused state unless when the simulate or the step functions are called. --- src/pycram/config/multiverse_conf.py | 9 +++++++++ src/pycram/worlds/multiverse.py | 29 +++++++++++++++++++--------- 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index f21c4cf2f..b27d06475 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -7,7 +7,16 @@ simulation_time_step: float = 1e-2 simulation_frequency: int = int(1 / simulation_time_step) +use_bullet_mode: bool = True +""" +If True, the simulation will always be in paused state unless the simulate() function is called, this behaves +similar to bullet_world which uses the bullet physics engine. +""" + use_controller: bool = False +""" +Only used when use_bullet_mode is False. This turns on the controller for the robot joints. +""" job_handling: conf.JobHandling = conf.JobHandling(let_pycram_move_attached_objects=False, let_pycram_handle_spawning=False) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b05fe19ec..a4e409f99 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,4 +1,3 @@ -import datetime import logging import os from time import sleep @@ -53,7 +52,13 @@ class Multiverse(World): the multiverse configuration file). """ - use_controller: bool = conf.use_controller + use_bullet_mode: bool = conf.use_bullet_mode + """ + If True, the simulation will always be in paused state unless the simulate() function is called, this behaves + similar to bullet_world which uses the bullet physics engine. + """ + + use_controller: bool = conf.use_controller and not use_bullet_mode """ Whether to use the controller for the robot joints or not. """ @@ -104,7 +109,8 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if not self.is_prospection_world: self._spawn_floor() - self.api_requester.pause_simulation() + if self.use_bullet_mode: + self.api_requester.pause_simulation() def _init_clients(self): self.reader: MultiverseReader = self.client_manager.create_reader( @@ -268,7 +274,7 @@ def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -310,7 +316,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): - self.set_mobile_robot_pose(obj ,pose) + self.set_mobile_robot_pose(obj, pose) else: self._set_body_pose(obj.name, pose) @@ -441,7 +447,8 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - self.simulate(0.1) + if self.use_bullet_mode: + self.simulate(0.1) multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -516,9 +523,13 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L return results def step(self): - self.api_requester.unpause_simulation() - sleep(self.simulation_time_step) - self.api_requester.pause_simulation() + """ + Perform a simulation step in the simulator, this is useful when use_bullet_mode is True. + """ + if self.use_bullet_mode: + self.api_requester.unpause_simulation() + sleep(self.simulation_time_step) + self.api_requester.pause_simulation() def save_physics_simulator_state(self) -> int: logging.warning("save_physics_simulator_state is not implemented in Multiverse") From fa947b4b6c736c0b1e07fc58bd2895589a204301 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 2 Aug 2024 12:12:07 +0200 Subject: [PATCH 108/189] [MultiverseAction] Now relative velocity is always sent as zeros for better stability when setting poses (useful in when bullet_mode is turned off) --- src/pycram/config/multiverse_conf.py | 2 +- src/pycram/worlds/multiverse.py | 3 ++- src/pycram/worlds/multiverse_communication/clients.py | 9 ++++++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index b27d06475..b357fd996 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -13,7 +13,7 @@ similar to bullet_world which uses the bullet physics engine. """ -use_controller: bool = False +use_controller: bool = True """ Only used when use_bullet_mode is False. This turns on the controller for the robot joints. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a4e409f99..121d64015 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -357,7 +357,8 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: """ self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: - self.xyzw_to_wxyz(pose.orientation_as_list())} + self.xyzw_to_wxyz(pose.orientation_as_list()), + MultiverseBodyProperty.RELATIVE_VELOCITY: [0.0]*6} for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 912f8fae2..76f21feae 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -318,8 +318,10 @@ def spawn_robot_with_actuators(self, robot_name: str, position: List[float], ori :param orientation: The orientation of the robot. :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. """ - send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value]} - data = [self.sim_time, *position, *orientation] + send_meta_data = {robot_name: [BodyProperty.POSITION.value, BodyProperty.ORIENTATION.value, + BodyProperty.RELATIVE_VELOCITY.value]} + relative_velocity = [0.0] * 6 + data = [self.sim_time, *position, *orientation, *relative_velocity] self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands) def _reset_request_meta_data(self): @@ -343,7 +345,8 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List """ self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position, - BodyProperty.ORIENTATION: orientation}) + BodyProperty.ORIENTATION: orientation, + BodyProperty.RELATIVE_VELOCITY: [0.0] * 6}) def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: """ From e430977189d3cb961fddce38d774bd0fbf66d1cd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 2 Aug 2024 18:16:32 +0200 Subject: [PATCH 109/189] [MultiverseAction] Pr2 is now available and working!!! Did some cleaning. Added virtual joints to pr2 for move base. Added tiago test case. --- .../robot_descriptions/pr2_description.py | 7 +++- src/pycram/world_concepts/world_object.py | 3 +- src/pycram/worlds/multiverse.py | 32 ++++++++--------- .../worlds/multiverse_datastructures/enums.py | 35 +++++++++++++------ .../worlds/multiverse_extras/helpers.py | 1 + test/bullet_world_testcase.py | 18 ++++++++++ test/test_action_designator.py | 6 +++- test/test_multiverse.py | 15 ++++---- 8 files changed, 77 insertions(+), 40 deletions(-) diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 402125f2a..3664cea49 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -1,13 +1,18 @@ +from ..datastructures.dataclasses import VirtualMoveBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import Arms, Grasp, GripperState, GripperType import rospkg +from ..worlds.multiverse_extras.helpers import get_robot_mjcf_path + rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' +mjcf_filename = get_robot_mjcf_path("willow_garage", "pr2") + pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", - filename) + filename, virtual_move_base_joints=VirtualMoveBaseJoints(), mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link", diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 4a8a26156..f30335d59 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -930,13 +930,12 @@ def reset_all_joints_positions(self) -> None: """ Set the current position of all joints to 0. This is useful if the joints should be reset to their default """ - joint_names = [joint.name for joint in self.joints.values()] # if not joint.is_virtual] + joint_names = [joint.name for joint in self.joints.values()] if len(joint_names) == 0: return joint_positions = [0] * len(joint_names) self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ Set the position of the given joint to the given joint pose and updates the poses of all attached objects. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 121d64015..81b10b4f0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -8,7 +8,8 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI -from .multiverse_datastructures.enums import MultiverseJointProperty, MultiverseBodyProperty +from .multiverse_datastructures.enums import MultiverseBodyProperty, MultiverseJointPosition, \ + MultiverseJointCMD from .multiverse_extras.helpers import find_multiverse_resources_path from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType @@ -28,19 +29,11 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ - supported_joint_types = (JointType.REVOLUTE, JointType.PRISMATIC) + supported_joint_types = (JointType.REVOLUTE, JointType.CONTINUOUS, JointType.PRISMATIC) """ A Tuple for the supported pycram joint types in Multiverse. """ - _joint_type_to_cmd_name: Dict[JointType, MultiverseJointProperty] = { - JointType.REVOLUTE: MultiverseJointProperty.REVOLUTE_JOINT_CMD, - JointType.PRISMATIC: MultiverseJointProperty.PRISMATIC_JOINT_CMD, - } - """ - A dictionary to map JointType to the corresponding multiverse joint command attribute name. - """ - added_multiverse_resources: bool = False """ A flag to check if the multiverse resources have been added. @@ -154,8 +147,8 @@ def _spawn_floor(self): world=self) @staticmethod - def get_joint_position_name(joint: Joint) -> MultiverseJointProperty: - return MultiverseJointProperty.from_pycram_joint_type(joint.type) + def get_joint_position_name(joint: Joint) -> MultiverseJointPosition: + return MultiverseJointPosition.from_pycram_joint_type(joint.type) def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: """ @@ -292,8 +285,9 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str if data is not None: return {name: list(value.values())[0][0] for name, value in data.items()} - def get_joint_cmd_name(self, joint_type: JointType) -> MultiverseJointProperty: - return self._joint_type_to_cmd_name[joint_type] + @staticmethod + def get_joint_cmd_name(joint_type: JointType) -> MultiverseJointCMD: + return MultiverseJointCMD.from_pycram_joint_type(joint_type) def get_link_pose(self, link: Link) -> Optional[Pose]: return self._get_body_pose(link.name) @@ -449,7 +443,8 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: Note: Currently Multiverse only gets one contact point per contact objects. """ if self.use_bullet_mode: - self.simulate(0.1) + # self.simulate(0.01) + pass multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -528,9 +523,10 @@ def step(self): Perform a simulation step in the simulator, this is useful when use_bullet_mode is True. """ if self.use_bullet_mode: - self.api_requester.unpause_simulation() - sleep(self.simulation_time_step) - self.api_requester.pause_simulation() + # self.api_requester.unpause_simulation() + # sleep(self.simulation_time_step) + # self.api_requester.pause_simulation() + pass def save_physics_simulator_state(self) -> int: logging.warning("save_physics_simulator_state is not implemented in Multiverse") diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index 152c80280..d6028a46a 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -33,23 +33,38 @@ class MultiverseBodyProperty(MultiverseProperty): class MultiverseJointProperty(MultiverseProperty): + pass + + +class MultiverseJointPosition(MultiverseJointProperty): """ - Enum for the different properties of a joint the Multiverse. + Enum for the Position names of the different joint types in the Multiverse. """ REVOLUTE_JOINT_POSITION = "joint_rvalue" PRISMATIC_JOINT_POSITION = "joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointPosition': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointPosition.REVOLUTE_JOINT_POSITION + elif joint_type == JointType.PRISMATIC: + return MultiverseJointPosition.PRISMATIC_JOINT_POSITION + else: + raise UnsupportedJointType(joint_type) + + +class MultiverseJointCMD(MultiverseJointProperty): + """ + Enum for the Command names of the different joint types in the Multiverse. + """ REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" - def is_cmd(self): - return self in [MultiverseJointProperty.REVOLUTE_JOINT_CMD, - MultiverseJointProperty.PRISMATIC_JOINT_CMD] - @classmethod - def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointProperty': - if joint_type == JointType.REVOLUTE: - return MultiverseJointProperty.REVOLUTE_JOINT_POSITION + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointCMD': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointCMD.REVOLUTE_JOINT_CMD elif joint_type == JointType.PRISMATIC: - return MultiverseJointProperty.PRISMATIC_JOINT_POSITION + return MultiverseJointCMD.PRISMATIC_JOINT_CMD else: - raise UnsupportedJointType(joint_type) \ No newline at end of file + raise UnsupportedJointType(joint_type) diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py index 3d2e0284d..049496c6d 100644 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -23,6 +23,7 @@ def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: return joint_actuators + def get_robot_mjcf_path(company_name: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: """ Get the path to the MJCF file of a robot. diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 54a48922e..b77103dfe 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -52,6 +52,24 @@ def tearDownClass(cls): cls.world.exit() +class TiagoBulletWorldTestCase(BulletWorldTestCase): + + @classmethod + def setUpClass(cls): + rdm = RobotDescriptionManager() + rdm.load_description("tiago_dual") + cls.world = BulletWorld(mode=WorldMode.DIRECT) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + RobotDescription.current_robot_description.name + cls.extension) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) + cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + ProcessModule.execution_delay = False + cls.viz_marker_publisher = VizMarkerPublisher() + OntologyManager(SOMA_ONTOLOGY_IRI) + + class BulletWorldGUITestCase(unittest.TestCase): world: BulletWorld diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 0d52f56aa..b2ee7fa7c 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -8,7 +8,7 @@ from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, Arms, GripperState, Grasp -from bullet_world_testcase import BulletWorldTestCase +from bullet_world_testcase import BulletWorldTestCase, TiagoBulletWorldTestCase import numpy as np @@ -164,5 +164,9 @@ def test_facing(self): self.assertAlmostEqual(milk_in_robot_frame.position.y, 0.) +class TiagoTestActionDesignatorGrounding(TiagoBulletWorldTestCase, TestActionDesignatorGrounding): + pass + + if __name__ == '__main__': unittest.main() diff --git a/test/test_multiverse.py b/test/test_multiverse.py index d7ba63b97..65d970705 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -76,11 +76,10 @@ def test_get_actuator_for_joint(self): def test_demo(self): extension = ObjectDescription.get_file_extension() - - robot = self.spawn_robot(robot_name='tiago_dual', position=[1.3, 2, 0.01]) + robot = self.spawn_robot(robot_name='pr2', position=[1.3, 2, 0.01]) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") - milk = Object("milk_box", ObjectType.MILK, f"milk_box{extension}", pose=Pose([2.4, 2, 1.02]), + milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) # bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), # color=Color(1, 1, 0, 1)) @@ -91,17 +90,17 @@ def test_demo(self): pick_pose = Pose([2.6, 2.15, 1]) - robot_desig = BelieveObject(names=["tiago_dual"]) - apartment_desig = BelieveObject(names=["apartment"]) + robot_desig = BelieveObject(names=[robot.name]) + apartment_desig = BelieveObject(names=[apartment.name]) with simulated_robot: ParkArmsAction([Arms.BOTH]).resolve().perform() - MoveTorsoAction([0.3]).resolve().perform() + MoveTorsoAction([0.25]).resolve().perform() milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) - # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() @staticmethod @with_simulated_robot @@ -187,7 +186,7 @@ def test_spawn_robot(self): if self.multiverse.robot is not None: robot = self.multiverse.robot else: - robot = self.spawn_robot() + robot = self.spawn_robot(robot_name="pr2") self.assertIsInstance(robot, Object) self.assertTrue(robot in self.multiverse.objects) self.assertTrue(self.multiverse.robot.name == robot.name) From 4600c00cf3ce75bffb8aef101ea40b4846059f90 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 2 Aug 2024 21:38:50 +0200 Subject: [PATCH 110/189] [MultiverseAction] Fixed one issure related to confusion of prospection vs real world robots. There is a problem in multiverse when setting robot move base joints, it confuses the prospection and real robots. --- src/pycram/datastructures/world.py | 14 ++++++++------ src/pycram/designators/action_designator.py | 2 +- src/pycram/world_concepts/world_object.py | 1 - .../worlds/multiverse_communication/clients.py | 10 +--------- test/test_multiverse.py | 12 ++++++++++++ 5 files changed, 22 insertions(+), 17 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 247000b33..185c18b65 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -623,21 +623,23 @@ def set_mobile_robot_pose(self, robot_obj: Object, pose: Pose) -> None: :param robot_obj: The robot object. :param pose: The target pose. """ - goal = self.get_move_base_joint_goal(pose) + goal = self.get_move_base_joint_goal(robot_obj, pose) robot_obj.set_multiple_joint_positions(goal) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + def get_move_base_joint_goal(self, robot_obj: Object, pose: Pose) -> Dict[str, float]: """ Get the goal for the move base joints of a mobile robot to reach a target pose. - param pose: The target pose. + :param robot_obj: The robot object. + :param pose: The target pose. return: The goal for the move base joints. """ - position_diff = self.get_position_diff(self.robot.get_position_as_list(), pose.position_as_list())[:2] + position_diff = self.get_position_diff(robot_obj.get_position_as_list(), pose.position_as_list()) + target_x, target_y = position_diff[0], position_diff[1] target_angle = self.get_z_angle(pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() - return {move_base_joints.translation_x: position_diff[0], - move_base_joints.translation_y: position_diff[1], + return {move_base_joints.translation_x: target_x, + move_base_joints.translation_y: target_y, move_base_joints.angular_z: target_angle} def get_move_base_joints(self) -> VirtualMoveBaseJoints: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index d14ad54f2..efdd8e59a 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -860,7 +860,7 @@ def perform(self) -> None: ParkArmsActionPerformable(Arms.BOTH).perform() pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), reachable_arm=self.arm) - # Tries to find a pick-up posotion for the robot that uses the given arm + # Tries to find a pick-up position for the robot that uses the given arm pickup_pose = None for pose in pickup_loc: if self.arm in pose.reachable_arms: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index f30335d59..2c7c2d154 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -945,7 +945,6 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) - def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 76f21feae..885bbd774 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -30,6 +30,7 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = meta_data = MultiverseMetaData() meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + self.is_prospection_world = is_prospection_world super().__init__(port=str(port), meta_data=meta_data) self.simulation_wait_time_factor = simulation_wait_time_factor self.run() @@ -421,15 +422,6 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, self.send_and_receive_data() return self.response_meta_data - @staticmethod - def get_actuator_name(body_name: str) -> str: - """ - Get the actuator name from the body name. - param body_name: The name of the body. - return: The actuator name. - """ - return body_name.replace("_joint", "_actuator") - def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: """ Send metadata to the multiverse server and get the response. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 65d970705..b9885f9a8 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -228,6 +228,18 @@ def test_set_robot_orientation(self): quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) + def test_set_robot_pose(self): + self.spawn_robot() + new_position = [-3, -3, 0.001] + # rotate by 45 degrees without using euler angles + rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) + new_quaternion = quaternion_multiply(self.multiverse.robot.get_orientation_as_list(), rotation_quaternion) + new_pose = Pose(new_position, new_quaternion) + self.multiverse.robot.set_pose(new_pose) + robot_pose = self.multiverse.robot.get_pose() + self.assert_poses_are_equal(new_pose, robot_pose, position_delta=self.multiverse.acceptable_position_error, + orientation_delta=self.multiverse.acceptable_orientation_error) + def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) cup = self.spawn_cup([1, 1.1, 0.1]) From 2b3b5a2de35cf2cdc617e715d6cd1f5fe32d967b Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 4 Aug 2024 13:08:47 +0200 Subject: [PATCH 111/189] [Multiverse] transport action is now working with Pr2 in Multiverse. --- src/pycram/datastructures/world.py | 28 ++++++++++++++--- src/pycram/datastructures/world_entity.py | 2 +- src/pycram/validation/error_checkers.py | 2 ++ src/pycram/worlds/multiverse.py | 4 --- test/test_multiverse.py | 38 ++++++++++++++++------- 5 files changed, 53 insertions(+), 21 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 185c18b65..b91bf86a8 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -9,7 +9,7 @@ import numpy as np import rospy from geometry_msgs.msg import Point -from tf.transformations import euler_from_quaternion +from tf.transformations import euler_from_quaternion, quaternion_multiply, quaternion_from_euler from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union from ..cache_manager import CacheManager @@ -633,15 +633,27 @@ def get_move_base_joint_goal(self, robot_obj: Object, pose: Pose) -> Dict[str, f :param pose: The target pose. return: The goal for the move base joints. """ - position_diff = self.get_position_diff(robot_obj.get_position_as_list(), pose.position_as_list()) + position_diff = self.get_position_diff(robot_obj.original_pose.position_as_list(), pose.position_as_list()) target_x, target_y = position_diff[0], position_diff[1] - target_angle = self.get_z_angle(pose.orientation_as_list()) + target_angle = self.get_z_angle_difference(robot_obj.original_pose.orientation_as_list(), + pose.orientation_as_list()) # Get the joints of the base link move_base_joints = self.get_move_base_joints() return {move_base_joints.translation_x: target_x, move_base_joints.translation_y: target_y, move_base_joints.angular_z: target_angle} + def get_z_angle_difference(self, original_orientation: List[float], target_orientation: List[float]) -> float: + """ + Get the difference between two z angles. + param original_orientation: The original orientation. + param target_orientation: The target orientation. + return: The difference between the two z angles. + """ + # rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) + # new_quaternion = quaternion_multiply(original_orientation, rotation_quaternion) + return self.get_z_angle(target_orientation) - self.get_z_angle(original_orientation) + def get_move_base_joints(self) -> VirtualMoveBaseJoints: """ Get the move base joints of the robot. @@ -1284,7 +1296,7 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :param parent_link_id: The id of the link to which the text should be attached. :return: The id of the added text. """ - raise NotImplementedError + rospy.logwarn(f"add_text is not implemented in {self.__class__.__name__}") def remove_text(self, text_id: Optional[int] = None) -> None: """ @@ -1292,7 +1304,7 @@ def remove_text(self, text_id: Optional[int] = None) -> None: :param text_id: The id of the text to be removed. """ - raise NotImplementedError + rospy.logwarn(f"remove_text is not implemented in {self.__class__.__name__}") def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: """ @@ -1348,6 +1360,12 @@ def resume_world_sync(self) -> None: """ self.world_sync.sync_lock.release() + def add_vis_axis(self, pose: Pose) -> None: + rospy.logwarn(f"add_vis_axis is not implemented in {self.__class__.__name__}") + + def remove_vis_axis(self) -> None: + rospy.logwarn(f"remove_vis_axis is not implemented in {self.__class__.__name__}") + def __del__(self): self.exit() diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index c21ae782a..e9b2a60ac 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -1,6 +1,6 @@ from abc import ABC, abstractmethod -from typing_extensions import Optional, TYPE_CHECKING, Dict +from typing_extensions import TYPE_CHECKING, Dict from .dataclasses import State diff --git a/src/pycram/validation/error_checkers.py b/src/pycram/validation/error_checkers.py index c78e18c7a..34f596330 100644 --- a/src/pycram/validation/error_checkers.py +++ b/src/pycram/validation/error_checkers.py @@ -242,6 +242,8 @@ def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float] """ quat_diff = calculate_quaternion_difference(quat_1, quat_2) quat_diff_angle = 2 * np.arctan2(np.linalg.norm(quat_diff[0:3]), quat_diff[3]) + if quat_diff_angle > np.pi: + quat_diff_angle = 2 * np.pi - quat_diff_angle return quat_diff_angle diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 81b10b4f0..d2803e58b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -567,7 +567,3 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def check_object_exists_in_multiverse(self, obj: Object) -> bool: return self.api_requester.check_object_exists(obj) - - @staticmethod - def add_vis_axis(pose: Pose) -> None: - logging.warning("add_vis_axis is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index b9885f9a8..ce4ffc2f3 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -207,9 +207,10 @@ def test_respawn_robot(self): self.assertTrue(self.multiverse.robot in self.multiverse.objects) def test_set_robot_position(self): + step = -1 for i in range(3): self.spawn_robot() - new_position = [-3, -3, 0.001] + new_position = [-3 + step*i, -3 + step*i, 0.001] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], @@ -229,16 +230,28 @@ def test_set_robot_orientation(self): self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) def test_set_robot_pose(self): - self.spawn_robot() - new_position = [-3, -3, 0.001] - # rotate by 45 degrees without using euler angles - rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) - new_quaternion = quaternion_multiply(self.multiverse.robot.get_orientation_as_list(), rotation_quaternion) - new_pose = Pose(new_position, new_quaternion) - self.multiverse.robot.set_pose(new_pose) - robot_pose = self.multiverse.robot.get_pose() - self.assert_poses_are_equal(new_pose, robot_pose, position_delta=self.multiverse.acceptable_position_error, - orientation_delta=self.multiverse.acceptable_orientation_error) + self.spawn_robot(orientation=quaternion_from_euler(0, 0, np.pi / 4)) + position_step = -1 + angle_step = np.pi / 4 + num_steps = 10 + self.step_robot_pose(self.multiverse.robot, position_step, angle_step, num_steps) + position_step = 1 + angle_step = -np.pi / 4 + self.step_robot_pose(self.multiverse.robot, position_step, angle_step, num_steps) + + def step_robot_pose(self, robot, position_step, angle_step, num_steps): + original_position = robot.get_position_as_list() + original_orientation = robot.get_orientation_as_list() + for i in range(num_steps): + new_position = [original_position[0] + position_step * (i + 1), + original_position[1] + position_step * (i + 1), original_position[2]] + rotation_quaternion = quaternion_from_euler(0, 0, angle_step * (i + 1)) + new_quaternion = quaternion_multiply(original_orientation, rotation_quaternion) + new_pose = Pose(new_position, new_quaternion) + self.multiverse.robot.set_pose(new_pose) + robot_pose = self.multiverse.robot.get_pose() + self.assert_poses_are_equal(new_pose, robot_pose, position_delta=self.multiverse.acceptable_position_error, + orientation_delta=self.multiverse.acceptable_orientation_error) def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) @@ -346,10 +359,13 @@ def spawn_milk(position: List, orientation: Optional[List] = None) -> Object: return milk def spawn_robot(self, position: Optional[List[float]] = None, + orientation: Optional[List[float]] = None, robot_name: Optional[str] = 'tiago_dual', replace: Optional[bool] = True) -> Object: if position is None: position = [-2, -2, 0.001] + if orientation is None: + orientation = [0, 0, 0, 1] if self.multiverse.robot is None or replace: if self.multiverse.robot is not None: self.multiverse.robot.remove() From b431d079bbe0f9a5f08234ad11b7899b2cda7f8d Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 4 Aug 2024 13:47:05 +0200 Subject: [PATCH 112/189] [Multiverse] tests are running. --- test/bullet_world_testcase.py | 18 ------------------ test/test_action_designator.py | 10 +--------- 2 files changed, 1 insertion(+), 27 deletions(-) diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index b77103dfe..54a48922e 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -52,24 +52,6 @@ def tearDownClass(cls): cls.world.exit() -class TiagoBulletWorldTestCase(BulletWorldTestCase): - - @classmethod - def setUpClass(cls): - rdm = RobotDescriptionManager() - rdm.load_description("tiago_dual") - cls.world = BulletWorld(mode=WorldMode.DIRECT) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, - RobotDescription.current_robot_description.name + cls.extension) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) - cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) - ProcessModule.execution_delay = False - cls.viz_marker_publisher = VizMarkerPublisher() - OntologyManager(SOMA_ONTOLOGY_IRI) - - class BulletWorldGUITestCase(unittest.TestCase): world: BulletWorld diff --git a/test/test_action_designator.py b/test/test_action_designator.py index b2ee7fa7c..a090e5fa2 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -8,7 +8,7 @@ from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, Arms, GripperState, Grasp -from bullet_world_testcase import BulletWorldTestCase, TiagoBulletWorldTestCase +from bullet_world_testcase import BulletWorldTestCase import numpy as np @@ -162,11 +162,3 @@ def test_facing(self): FaceAtPerformable(self.milk.pose).perform() milk_in_robot_frame = LocalTransformer().transform_to_object_frame(self.milk.pose, self.robot) self.assertAlmostEqual(milk_in_robot_frame.position.y, 0.) - - -class TiagoTestActionDesignatorGrounding(TiagoBulletWorldTestCase, TestActionDesignatorGrounding): - pass - - -if __name__ == '__main__': - unittest.main() From 4248cfbe15df25132743f61adb70316128886a11 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 5 Aug 2024 12:34:07 +0200 Subject: [PATCH 113/189] [Multiverse] changed place location of transport demo. --- test/test_multiverse.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ce4ffc2f3..7ca742741 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -100,7 +100,7 @@ def test_demo(self): milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) - TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() @staticmethod @with_simulated_robot From 84eee01d2e107b99b25c89368b03e3e7157bd9bd Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 6 Aug 2024 10:02:07 +0200 Subject: [PATCH 114/189] [Multiverse] flexible name for robot description addition condition in attachment equalization. --- src/pycram/designators/object_designator.py | 1 + src/pycram/robot_description.py | 7 ++++--- src/pycram/world_concepts/constraints.py | 1 + 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 85d090499..d94a77ed2 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -14,6 +14,7 @@ if TYPE_CHECKING: import owlready2 + class BelieveObject(ObjectDesignatorDescription): """ Description for Objects that are only believed in. diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index e6af1aaad..22d7ce52b 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -40,9 +40,10 @@ def load_description(self, name: str): :param name: Name of the robot to which the description should be loaded. :return: The loaded robot description. """ - if name in self.descriptions.keys(): - RobotDescription.current_robot_description = self.descriptions[name] - return self.descriptions[name] + for key in self.descriptions.keys(): + if key in name.lower(): + RobotDescription.current_robot_description = self.descriptions[key] + return self.descriptions[key] else: rospy.logerr(f"Robot description {name} not found") diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 5843db56e..7da4ace8c 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -324,6 +324,7 @@ def __eq__(self, other): return (self.parent_link.name == other.parent_link.name and self.child_link.name == other.child_link.name and self.bidirectional == other.bidirectional + and self.loose == other.loose and np.allclose(self.parent_to_child_transform.translation_as_list(), other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-4) and np.allclose(self.parent_to_child_transform.rotation_as_list(), From e97ac8d688804da3d8ff03eea26aa806ef3422b6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 12:55:53 +0200 Subject: [PATCH 115/189] [MultiverseAction] corrected robot description dictionary name confusion issue. --- src/pycram/robot_description.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 22d7ce52b..5f892a291 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -40,12 +40,16 @@ def load_description(self, name: str): :param name: Name of the robot to which the description should be loaded. :return: The loaded robot description. """ - for key in self.descriptions.keys(): - if key in name.lower(): - RobotDescription.current_robot_description = self.descriptions[key] - return self.descriptions[key] + if name in self.descriptions.keys(): + RobotDescription.current_robot_description = self.descriptions[name] + return self.descriptions[name] else: - rospy.logerr(f"Robot description {name} not found") + for key in self.descriptions.keys(): + if key in name.lower(): + RobotDescription.current_robot_description = self.descriptions[key] + return self.descriptions[key] + else: + rospy.logerr(f"Robot description {name} not found") def register_description(self, description: RobotDescription): """ From 25eb18bf1fcf8d240c7a2f3585785feb059d4ca2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 16:44:31 +0200 Subject: [PATCH 116/189] [MultiverseAction] Fixed detachment syncing problem. --- src/pycram/world_concepts/world_object.py | 28 +++++++++++++++++---- src/pycram/worlds/multiverse.py | 6 ++--- test/test_attachment.py | 30 ++++++++++------------- test/test_multiverse.py | 2 ++ 4 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 2c7c2d154..158caec32 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -13,10 +13,10 @@ ContactPointsList) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform -from ..exceptions import ObjectAlreadyExists from ..datastructures.world import World from ..datastructures.world_entity import WorldEntity from ..description import ObjectDescription, LinkDescription, Joint +from ..exceptions import ObjectAlreadyExists from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription @@ -707,14 +707,33 @@ def current_state(self, state: ObjectState) -> None: def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Set the attachments of this object to the given attachments. + :param attachments: A dictionary with the object as key and the attachment as value. + """ + self.detach_not_attached_objects(attachments) + self.attach_not_attached_objects(attachments) + + def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> None: + """ + Detach objects that are not in the attachments list and are in the current attachments list. + :param attachments: A dictionary with the object as key and the attachment as value. + """ + copy_of_attachments = self.attachments.copy() + for obj, attachment in copy_of_attachments.items(): + original_obj = obj + if self.world.is_prospection_world and len(attachments) > 0 \ + and not list(attachments.keys())[0].world.is_prospection_world: + obj = self.world.get_object_for_prospection_object(obj) + if obj not in attachments: + self.detach(original_obj) + def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> None: + """ + Attach objects that are in the given attachments list but not in the current attachments list. :param attachments: A dictionary with the object as key and the attachment as value. """ for obj, attachment in attachments.items(): if self.world.is_prospection_world and not obj.world.is_prospection_world: - # In case this object is in the prospection world and the other object is not, the attachment will not - # be set. - continue + obj = self.world.get_prospection_object_for_object(obj) if obj in self.attachments: if self.attachments[obj] != attachment: self.detach(obj) @@ -1204,7 +1223,6 @@ def copy_to_world(self, world: World) -> Object: """ obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), world, self.color) - obj.current_state = self.current_state return obj def __eq__(self, other): diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index d2803e58b..3e4fbf42e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -523,9 +523,9 @@ def step(self): Perform a simulation step in the simulator, this is useful when use_bullet_mode is True. """ if self.use_bullet_mode: - # self.api_requester.unpause_simulation() - # sleep(self.simulation_time_step) - # self.api_requester.pause_simulation() + self.api_requester.unpause_simulation() + sleep(self.simulation_time_step) + self.api_requester.pause_simulation() pass def save_physics_simulator_state(self) -> int: diff --git a/test/test_attachment.py b/test/test_attachment.py index cff5e78d6..b77965d5f 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -20,6 +20,19 @@ def test_detach(self): self.assertTrue(self.robot not in self.milk.attachments) self.assertTrue(self.milk not in self.robot.attachments) + def test_detach_sync_in_prospection_world(self): + self.milk.attach(self.robot) + with UseProspectionWorld(): + pass + self.milk.detach(self.robot) + with UseProspectionWorld(): + self.assertTrue(self.milk not in self.robot.attachments) + self.assertTrue(self.robot not in self.milk.attachments) + prospection_milk = self.world.get_prospection_object_for_object(self.milk) + prospection_robot = self.world.get_prospection_object_for_object(self.robot) + self.assertTrue(prospection_milk not in prospection_robot.attachments) + self.assertTrue(prospection_robot not in prospection_milk.attachments) + def test_attachment_behavior(self): self.robot.attach(self.milk) @@ -80,23 +93,6 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): self.world.remove_object(milk_2) self.world.remove_object(cereal_2) - def test_no_attachment_in_prospection_world(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) - - milk_2.attach(cereal_2) - - with UseProspectionWorld(): - prospection_milk = self.world.get_prospection_object_for_object(milk_2) - prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) - - self.assertTrue(prospection_milk.attachments == {}) - self.assertTrue(prospection_cereal.attachments == {}) - - self.world.remove_object(milk_2) - self.world.remove_object(cereal_2) - def test_attaching_to_robot_and_moving(self): self.robot.attach(self.milk) milk_pos = self.milk.get_position() diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 7ca742741..1b36b280c 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -311,6 +311,7 @@ def test_get_object_contact_points(self): self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) cup = self.spawn_cup([1, 1, 0.1]) + self.multiverse.simulate(0.1) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -322,6 +323,7 @@ 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.1]) + self.multiverse.simulate(0.1) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From fcd85263bc5b3251ca213118058d1f29abccf844 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 16:53:47 +0200 Subject: [PATCH 117/189] [MultiverseAction] Fixed failing test of attachment in real syncing to prospection but not vice versa. --- src/pycram/worlds/multiverse.py | 6 +----- test/test_attachment.py | 11 ++++++----- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 3e4fbf42e..60d5df3f3 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -436,15 +436,12 @@ def remove_constraint(self, constraint_id) -> None: self.api_requester.detach(constraint) def perform_collision_detection(self) -> None: - self.step() + pass def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ Note: Currently Multiverse only gets one contact point per contact objects. """ - if self.use_bullet_mode: - # self.simulate(0.01) - pass multiverse_contact_points = self.api_requester.get_contact_points(obj) contact_points = ContactPointsList([]) body_link = None @@ -526,7 +523,6 @@ def step(self): self.api_requester.unpause_simulation() sleep(self.simulation_time_step) self.api_requester.pause_simulation() - pass def save_physics_simulator_state(self) -> int: logging.warning("save_physics_simulator_state is not implemented in Multiverse") diff --git a/test/test_attachment.py b/test/test_attachment.py index b77965d5f..c91ff3ee0 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -70,22 +70,23 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): # self.assertTrue(cereal_2 not in prospection_milk.attachments) prospection_cereal = self.world.get_prospection_object_for_object(cereal_2) # self.assertTrue(prospection_cereal in prospection_milk.attachments) - self.assertTrue(prospection_milk.attachments == {}) + self.assertTrue(prospection_cereal in prospection_milk.attachments.keys()) # Assert that when prospection object is moved, the real object is not moved prospection_milk_pos = prospection_milk.get_position() cereal_pos = cereal_2.get_position() - prospection_cereal_pos = prospection_cereal.get_position() + estimated_prospection_cereal_pos = prospection_cereal.get_position() + estimated_prospection_cereal_pos.x += 1 # Move prospection milk object prospection_milk_pos.x += 1 prospection_milk.set_position(prospection_milk_pos) - # Prospection cereal should not move since it is not attached to prospection milk + # Prospection cereal should move since it is attached to prospection milk new_prospection_cereal_pose = prospection_cereal.get_position() - self.assertTrue(new_prospection_cereal_pose == prospection_cereal_pos) + self.assertTrue(new_prospection_cereal_pose == estimated_prospection_cereal_pos) - # Also Real cereal object should not move + # Also Real cereal object should not move since it is not affected by prospection milk new_cereal_pos = cereal_2.get_position() assumed_cereal_pos = cereal_pos self.assertTrue(new_cereal_pos == assumed_cereal_pos) From 8def0da98829e27a91d613526e957028568b8145 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 17:06:05 +0200 Subject: [PATCH 118/189] [MultiverseAction] made sure when setting attachments to obey the loose such that the correct attachment is set. --- src/pycram/world_concepts/world_object.py | 7 +++++-- test/bullet_world_testcase.py | 5 +++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 158caec32..d6274cf7e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -739,8 +739,11 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> self.detach(obj) else: continue - self.attach(obj, attachment.parent_link.name, attachment.child_link.name, - attachment.bidirectional) + if attachment.loose: + obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional) + else: + self.attach(obj, attachment.parent_link.name, attachment.child_link.name, + attachment.bidirectional) @property def link_states(self) -> Dict[int, LinkState]: diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 54a48922e..7e572500b 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -2,6 +2,7 @@ import unittest import pycram.tasktree +from pycram.datastructures.world import UseProspectionWorld from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose @@ -36,6 +37,8 @@ def setUpClass(cls): def setUp(self): self.world.reset_world() + with UseProspectionWorld(): + pass # DO NOT WRITE TESTS HERE!!! # Test related to the BulletWorld should be written in test_bullet_world.py @@ -45,6 +48,8 @@ def tearDown(self): pycram.tasktree.reset_tree() time.sleep(0.05) self.world.reset_world() + with UseProspectionWorld(): + pass @classmethod def tearDownClass(cls): From a099097fbeff3b4a8c974bc7c0af3ed3b44d7d7c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 18:36:00 +0200 Subject: [PATCH 119/189] [MultiverseAction] set all posses of all objects first before setting the attachments when syncing. --- src/pycram/datastructures/world.py | 4 ++++ src/pycram/world_concepts/world_object.py | 5 ++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b91bf86a8..288cf2842 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1530,6 +1530,10 @@ def sync_objects_states(self) -> None: """ Synchronizes the state of all objects in the World with the prospection world. """ + # Set the pose of the prospection objects to the pose of the world objects + for world_obj, prospection_obj in self.object_to_prospection_object_map.items(): + prospection_obj.set_pose(world_obj.pose, base=False, set_attachments=False) + # Set the rest of the state of the prospection objects to the state of the world objects for world_obj, prospection_obj in self.object_to_prospection_object_map.items(): prospection_obj.current_state = world_obj.current_state diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d6274cf7e..a41123915 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -724,7 +724,10 @@ def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> and not list(attachments.keys())[0].world.is_prospection_world: obj = self.world.get_object_for_prospection_object(obj) if obj not in attachments: - self.detach(original_obj) + if attachment.loose: + original_obj.detach(self) + else: + self.detach(original_obj) def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> None: """ From 079de32cd23d46ccdc6e50e5a28a9511698c4ed9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 19:12:21 +0200 Subject: [PATCH 120/189] [MultiverseAction] added is_inverse to indicate which was the original parent of the attachment. --- src/pycram/world_concepts/constraints.py | 19 +++++++------------ src/pycram/world_concepts/world_object.py | 9 ++++++--- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 7da4ace8c..9f504a840 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -202,9 +202,10 @@ class Attachment(AbstractConstraint): def __init__(self, parent_link: Link, child_link: Link, - bidirectional: Optional[bool] = False, + bidirectional: bool = False, parent_to_child_transform: Optional[Transform] = None, - constraint_id: Optional[int] = None): + constraint_id: Optional[int] = None, + is_inverse: bool = False): """ Creates an attachment between the parent object link and the child object link. This could be a bidirectional attachment, meaning that both objects will move when one moves. @@ -220,6 +221,7 @@ def __init__(self, self.id = constraint_id self.bidirectional: bool = bidirectional self._loose: bool = False + self.is_inverse: bool = is_inverse if parent_to_child_transform is not None: self.parent_to_child_transform = parent_to_child_transform @@ -283,7 +285,7 @@ def get_inverse(self) -> 'Attachment': :return: A new Attachment object with the parent and child links swapped. """ attachment = Attachment(self.child_link, self.parent_link, self.bidirectional, - constraint_id=self.id) + constraint_id=self.id, is_inverse=not self.is_inverse) attachment.loose = not self._loose return attachment @@ -303,13 +305,6 @@ def loose(self, loose: bool) -> None: """ self._loose = loose and not self.bidirectional - @property - def is_reversed(self) -> bool: - """ - :return: True if the parent and child links are swapped. - """ - return self.loose - def __del__(self) -> None: """ Removes the constraint between the parent and the child links if one exists when the attachment is deleted. @@ -326,9 +321,9 @@ def __eq__(self, other): and self.bidirectional == other.bidirectional and self.loose == other.loose and np.allclose(self.parent_to_child_transform.translation_as_list(), - other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-4) + other.parent_to_child_transform.translation_as_list(), rtol=0, atol=1e-3) and np.allclose(self.parent_to_child_transform.rotation_as_list(), - other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-4)) + other.parent_to_child_transform.rotation_as_list(), rtol=0, atol=1e-3)) def __hash__(self): return hash((self.parent_link.name, self.child_link.name, self.bidirectional, self.parent_to_child_transform)) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a41123915..2749e975d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -724,7 +724,7 @@ def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> and not list(attachments.keys())[0].world.is_prospection_world: obj = self.world.get_object_for_prospection_object(obj) if obj not in attachments: - if attachment.loose: + if attachment.is_inverse: original_obj.detach(self) else: self.detach(original_obj) @@ -739,10 +739,13 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> obj = self.world.get_prospection_object_for_object(obj) if obj in self.attachments: if self.attachments[obj] != attachment: - self.detach(obj) + if attachment.is_inverse: + obj.detach(self) + else: + self.detach(obj) else: continue - if attachment.loose: + if attachment.is_inverse: obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional) else: self.attach(obj, attachment.parent_link.name, attachment.child_link.name, From 681000d888117c9b3cb459b98dd4fe13dc01788b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 20:10:25 +0200 Subject: [PATCH 121/189] [MultiverseAction] corrected attachment frame names when syncing. --- src/pycram/datastructures/world.py | 12 +++++++----- src/pycram/world_concepts/world_object.py | 23 ++++++++++++++++------- src/pycram/worlds/multiverse.py | 22 +++++++++++++++++++--- test/test_attachment.py | 2 +- test/test_multiverse.py | 1 + 5 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 288cf2842..da5937212 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1531,11 +1531,13 @@ def sync_objects_states(self) -> None: Synchronizes the state of all objects in the World with the prospection world. """ # Set the pose of the prospection objects to the pose of the world objects - for world_obj, prospection_obj in self.object_to_prospection_object_map.items(): - prospection_obj.set_pose(world_obj.pose, base=False, set_attachments=False) - # Set the rest of the state of the prospection objects to the state of the world objects - for world_obj, prospection_obj in self.object_to_prospection_object_map.items(): - prospection_obj.current_state = world_obj.current_state + obj_pose_dict = {prospection_obj: obj.pose + for obj, prospection_obj in self.object_to_prospection_object_map.items()} + self.world.prospection_world.reset_multiple_objects_base_poses(obj_pose_dict) + for obj, prospection_obj in self.object_to_prospection_object_map.items(): + prospection_obj.set_attachments(obj.attachments) + prospection_obj.link_states = obj.link_states + prospection_obj.joint_states = obj.joint_states def check_for_equal(self) -> bool: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 2749e975d..fa0425044 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -506,7 +506,8 @@ def attach(self, parent_link: Optional[str] = None, child_link: Optional[str] = None, bidirectional: bool = True, - coincide_the_objects: bool = False) -> None: + coincide_the_objects: bool = False, + parent_to_child_transform: Optional[Transform] = None) -> None: """ Attach another object to this object. This is done by saving the transformation between the given link, if there is one, and @@ -522,15 +523,13 @@ def attach(self, :param child_link: The link name of the other object. :param bidirectional: If the attachment should be a loose attachment. :param coincide_the_objects: If True the object frames will be coincided. + :param parent_to_child_transform: The transform from the parent to the child object. """ parent_link = self.links[parent_link] if parent_link else self.root_link child_link = child_object.links[child_link] if child_link else child_object.root_link - if coincide_the_objects: + if coincide_the_objects and parent_to_child_transform is None: parent_to_child_transform = Transform() - else: - # parent_to_child_transform = parent_link.get_transform_to_link(child_link) - parent_to_child_transform = None attachment = Attachment(parent_link, child_link, bidirectional, parent_to_child_transform) self.attachments[child_object] = attachment @@ -737,6 +736,11 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> for obj, attachment in attachments.items(): if self.world.is_prospection_world and not obj.world.is_prospection_world: obj = self.world.get_prospection_object_for_object(obj) + # This variable indicates that the object is in the prospection world and the attachment is in the + # real world. + is_prospection = True + else: + is_prospection = False if obj in self.attachments: if self.attachments[obj] != attachment: if attachment.is_inverse: @@ -745,11 +749,16 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> self.detach(obj) else: continue + att_transform = attachment.parent_to_child_transform.copy() + if is_prospection: + att_transform.frame = self.prospection_world_prefix + att_transform.frame + att_transform.child_frame_id = self.prospection_world_prefix + att_transform.child_frame_id if attachment.is_inverse: - obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional) + obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional, + parent_to_child_transform=att_transform.invert()) else: self.attach(obj, attachment.parent_link.name, attachment.child_link.name, - attachment.bidirectional) + attachment.bidirectional, parent_to_child_transform=att_transform) @property def link_states(self) -> Dict[int, LinkState]: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 60d5df3f3..5b9c9ceaa 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -11,6 +11,7 @@ from .multiverse_datastructures.enums import MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from .multiverse_extras.helpers import find_multiverse_resources_path +from ..config import multiverse_conf as conf from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -21,7 +22,6 @@ validate_joint_position from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object -from ..config import multiverse_conf as conf class Multiverse(World): @@ -139,6 +139,16 @@ def _make_sure_multiverse_resources_are_added(self): World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True + def remove_multiverse_resources(self): + """ + Remove the multiverse resources from the pycram world resources. + """ + if self.added_multiverse_resources: + dirname = find_multiverse_resources_path() + World.data_directory.remove(dirname) + World.cache_manager.data_directory = World.data_directory + self.added_multiverse_resources = False + def _spawn_floor(self): """ Spawn the plane in the simulator. @@ -267,7 +277,7 @@ def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -334,6 +344,12 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None Reset the poses of multiple objects in the simulator. param objects: The dictionary of objects and poses. """ + for obj in objects.keys(): + if (obj.obj_type == ObjectType.ROBOT and + RobotDescription.current_robot_description.virtual_move_base_joints is not None): + self.set_mobile_robot_pose(obj, objects[obj]) + objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, + ObjectType.ROBOT]} self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) def _set_body_pose(self, body_name: str, pose: Pose) -> None: @@ -352,7 +368,7 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: self.xyzw_to_wxyz(pose.orientation_as_list()), - MultiverseBodyProperty.RELATIVE_VELOCITY: [0.0]*6} + MultiverseBodyProperty.RELATIVE_VELOCITY: [0.0] * 6} for name, pose in body_poses.items()}) def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: diff --git a/test/test_attachment.py b/test/test_attachment.py index c91ff3ee0..a6491eece 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -84,7 +84,7 @@ def test_prospection_object_attachments_not_changed_with_real_object(self): # Prospection cereal should move since it is attached to prospection milk new_prospection_cereal_pose = prospection_cereal.get_position() - self.assertTrue(new_prospection_cereal_pose == estimated_prospection_cereal_pos) + self.assertAlmostEqual(new_prospection_cereal_pose.x, estimated_prospection_cereal_pos.x, delta=0.01) # Also Real cereal object should not move since it is not affected by prospection milk new_cereal_pos = cereal_2.get_position() diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 1b36b280c..ad65aeeec 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -54,6 +54,7 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): cls.multiverse.exit() + cls.multiverse.remove_multiverse_resources() def tearDown(self): self.multiverse.reset_world_and_remove_objects() From 8728810bc55360e0776a2fba78f892d7fb92f694 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 6 Aug 2024 20:58:17 +0200 Subject: [PATCH 122/189] [MultiverseAction] some cleaning. --- src/pycram/exceptions.py | 6 +++ src/pycram/world_concepts/world_object.py | 52 +++++++++++++++-------- 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/src/pycram/exceptions.py b/src/pycram/exceptions.py index 37666b398..8b2e237c1 100644 --- a/src/pycram/exceptions.py +++ b/src/pycram/exceptions.py @@ -23,3 +23,9 @@ class ObjectDescriptionNotFound(KeyError): def __init__(self, object_name: str, path: str, extension: str): super().__init__(f"{object_name} with path {path} and extension {extension} is not in supported extensions, and" f" the description data was not found on the ROS parameter server") + + +class WorldMismatchErrorBetweenObjects(Exception): + def __init__(self, obj_1: 'Object', obj_2: 'Object'): + super().__init__(f"World mismatch between the attached objects {obj_1.name} and {obj_2.name}," + f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") \ No newline at end of file diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index fa0425044..edbc0daa8 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -16,7 +16,7 @@ from ..datastructures.world import World from ..datastructures.world_entity import WorldEntity from ..description import ObjectDescription, LinkDescription, Joint -from ..exceptions import ObjectAlreadyExists +from ..exceptions import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription @@ -734,13 +734,9 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> :param attachments: A dictionary with the object as key and the attachment as value. """ for obj, attachment in attachments.items(): - if self.world.is_prospection_world and not obj.world.is_prospection_world: + is_prospection = self.world.is_prospection_world and not obj.world.is_prospection_world + if is_prospection: obj = self.world.get_prospection_object_for_object(obj) - # This variable indicates that the object is in the prospection world and the attachment is in the - # real world. - is_prospection = True - else: - is_prospection = False if obj in self.attachments: if self.attachments[obj] != attachment: if attachment.is_inverse: @@ -749,16 +745,38 @@ def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> self.detach(obj) else: continue - att_transform = attachment.parent_to_child_transform.copy() - if is_prospection: - att_transform.frame = self.prospection_world_prefix + att_transform.frame - att_transform.child_frame_id = self.prospection_world_prefix + att_transform.child_frame_id - if attachment.is_inverse: - obj.attach(self, attachment.child_link.name, attachment.parent_link.name, attachment.bidirectional, - parent_to_child_transform=att_transform.invert()) - else: - self.attach(obj, attachment.parent_link.name, attachment.child_link.name, - attachment.bidirectional, parent_to_child_transform=att_transform) + self.mimic_attachment_with_object(attachment, obj) + + def mimic_attachment_with_object(self, attachment: Attachment, child_object: Object) -> None: + """ + Mimic the given attachment for this and the given child objects. + :param attachment: The attachment to mimic. + :param child_object: The child object. + """ + att_transform = self.get_attachment_transform_with_object(attachment, child_object) + if attachment.is_inverse: + child_object.attach(self, attachment.child_link.name, attachment.parent_link.name, + attachment.bidirectional, + parent_to_child_transform=att_transform.invert()) + else: + self.attach(child_object, attachment.parent_link.name, attachment.child_link.name, + attachment.bidirectional, parent_to_child_transform=att_transform) + + def get_attachment_transform_with_object(self, attachment: Attachment, child_object: Object) -> Transform: + """ + Returns the attachment transform for the given parent and child objects, taking into account the prospection + world. + :param attachment: The attachment. + :param child_object: The child object. + :return: The attachment transform. + """ + if self.world != child_object.world: + raise WorldMismatchErrorBetweenObjects(self, child_object) + att_transform = attachment.parent_to_child_transform.copy() + if self.world.is_prospection_world and not attachment.parent_object.world.is_prospection_world: + att_transform.frame = self.prospection_world_prefix + att_transform.frame + att_transform.child_frame_id = self.prospection_world_prefix + att_transform.child_frame_id + return att_transform @property def link_states(self) -> Dict[int, LinkState]: From a9aa1e0e1c6c717ac7dd4591bde1a2a48de77600 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 7 Aug 2024 10:05:58 +0200 Subject: [PATCH 123/189] [MultiverseAction] renaming of methods. --- src/pycram/world_concepts/world_object.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index edbc0daa8..9a3c0b237 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -708,10 +708,10 @@ def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: Set the attachments of this object to the given attachments. :param attachments: A dictionary with the object as key and the attachment as value. """ - self.detach_not_attached_objects(attachments) - self.attach_not_attached_objects(attachments) + self.detach_objects_not_in_attachments(attachments) + self.attach_objects_in_attachments(attachments) - def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> None: + def detach_objects_not_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Detach objects that are not in the attachments list and are in the current attachments list. :param attachments: A dictionary with the object as key and the attachment as value. @@ -728,7 +728,7 @@ def detach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> else: self.detach(original_obj) - def attach_not_attached_objects(self, attachments: Dict[Object, Attachment]) -> None: + def attach_objects_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Attach objects that are in the given attachments list but not in the current attachments list. :param attachments: A dictionary with the object as key and the attachment as value. From 7b1afe852d8ca9415637a2d3bd2d37cf6f9ac25c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 7 Aug 2024 16:29:05 +0200 Subject: [PATCH 124/189] [MultiverseAction] Implemented get_images_for_target for multiverse, corrected camera frame in robot_description.py. --- src/pycram/datastructures/pose.py | 11 +++ src/pycram/description.py | 21 ----- src/pycram/exceptions.py | 7 +- src/pycram/local_transformer.py | 43 +++++++--- src/pycram/robot_description.py | 2 +- src/pycram/worlds/multiverse.py | 137 +++++++++++++++++++++++++++++- test/test_multiverse.py | 31 +++++-- 7 files changed, 205 insertions(+), 47 deletions(-) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 4a7796453..ea5860e2b 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -345,6 +345,17 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional self.frame = frame + def get_homogeneous_matrix(self) -> np.ndarray: + """ + Returns the homogeneous matrix of this Transform. The matrix can be used to transform points from the frame_id + to the child_frame_id. + + :return: The homogeneous matrix of this Transform + """ + translation = transformations.translation_matrix(self.translation_as_list()) + rotation = transformations.quaternion_matrix(self.rotation_as_list()) + return np.dot(translation, rotation) + @classmethod def from_pose_and_child_frame(cls, pose: Pose, child_frame_name: str) -> Transform: return cls(pose.position_as_list(), pose.orientation_as_list(), pose.frame, child_frame_name, diff --git a/src/pycram/description.py b/src/pycram/description.py index 43be29359..2d2517066 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -235,27 +235,6 @@ def get_transform_from_root_link(self) -> Transform: def get_transform_to_root_link(self) -> Transform: return self.get_transform_to_link(self.object.root_link) - def set_pose(self, pose: Pose) -> None: - """ - Sets the pose of this link to the given pose. - NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints - that can allow the link to be at the given pose. - :param pose: The target pose for this link. - """ - self.object.set_pose(self.get_object_pose_given_link_pose(pose)) - - def get_object_pose_given_link_pose(self, pose): - return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() - - def get_pose_given_object_pose(self, pose): - return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() - - def get_transform_from_root_link(self) -> Transform: - return self.get_transform_from_link(self.object.root_link) - - def get_transform_to_root_link(self) -> Transform: - return self.get_transform_to_link(self.object.root_link) - @property def current_state(self) -> LinkState: return LinkState(self.constraint_ids.copy()) diff --git a/src/pycram/exceptions.py b/src/pycram/exceptions.py index 8b2e237c1..51e6b2097 100644 --- a/src/pycram/exceptions.py +++ b/src/pycram/exceptions.py @@ -28,4 +28,9 @@ def __init__(self, object_name: str, path: str, extension: str): class WorldMismatchErrorBetweenObjects(Exception): def __init__(self, obj_1: 'Object', obj_2: 'Object'): super().__init__(f"World mismatch between the attached objects {obj_1.name} and {obj_2.name}," - f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") \ No newline at end of file + f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") + + +class ObjectFrameNotFoundError(KeyError): + def __init__(self, frame_name: str): + super().__init__(f"Frame {frame_name} does not belong to any of the objects in the world.") \ No newline at end of file diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 673c6e67f..91f07b527 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -1,6 +1,8 @@ import sys import logging +from .exceptions import ObjectFrameNotFoundError + if 'world' in sys.modules: logging.warning("(publisher) Make sure that you are not loading this module from pycram.world.") import rospy @@ -65,18 +67,14 @@ def transform_to_object_frame(self, pose: Pose, target_frame = world_object.tf_frame return self.transform_pose(pose, target_frame) - def update_transforms_for_objects(self, source_object_name: str, target_object_name: str) -> None: + def update_transforms_for_objects(self, object_names: List[str]) -> None: """ Updates the transforms for objects affected by the transformation. The objects are identified by their names. - :param source_object_name: Name of the object of the source frame - :param target_object_name: Name of the object of the target frame + :param object_names: List of object names for which the transforms should be updated """ - source_object = self.world.get_object_by_name(source_object_name) - target_object = self.world.get_object_by_name(target_object_name) - for obj in {source_object, target_object}: - if obj: - obj.update_link_transforms() + objects = list(map(self.world.get_object_by_name, object_names)) + [obj.update_link_transforms() for obj in objects] def transform_pose(self, pose: Pose, target_frame: str) -> Optional[Pose]: """ @@ -86,8 +84,8 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Optional[Pose]: :param target_frame: Name of the TF frame into which the Pose should be transformed :return: A transformed pose in the target frame """ - self.update_transforms_for_objects(self.get_object_name_for_frame(pose.frame), - self.get_object_name_for_frame(target_frame)) + objects = list(map(self.get_object_name_for_frame, [pose.frame, target_frame])) + self.update_transforms_for_objects([obj for obj in objects if obj is not None]) copy_pose = pose.copy() copy_pose.header.stamp = rospy.Time(0) @@ -104,14 +102,31 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Optional[Pose]: return Pose(*copy_pose.to_list(), frame=new_pose.header.frame_id) - def get_object_name_for_frame(self, frame: str) -> str: + def get_object_name_for_frame(self, frame: str) -> Optional[str]: """ Returns the name of the object that is associated with the given frame. :param frame: The frame for which the object name should be returned :return: The name of the object associated with the frame """ - return frame.split("/")[0] + if frame == "map": + return None + obj_name = [obj.name for obj in self.world.objects if frame == obj.tf_frame] + return obj_name[0] if len(obj_name) > 0 else self.get_object_name_for_link_frame(frame) + + def get_object_name_for_link_frame(self, link_frame: str) -> str: + """ + Returns the name of the object that is associated with the given link frame. + + :param link_frame: The frame of the link for which the object name should be returned + :return: The name of the object associated with the link frame + """ + object_name = [obj.name for obj in self.world.objects for link in obj.links.values() + if link_frame in (link.name, link.tf_frame)] + if len(object_name) > 0: + return object_name[0] + else: + raise ObjectFrameNotFoundError(link_frame) def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: @@ -124,8 +139,8 @@ def lookup_transform_from_source_to_target_frame(self, source_frame: str, target :param time: Time at which the transform should be looked up :return: The transform from source_frame to target_frame """ - self.update_transforms_for_objects(self.get_object_name_for_frame(source_frame), - self.get_object_name_for_frame(target_frame)) + objects = list(map(self.get_object_name_for_frame, [source_frame, target_frame])) + self.update_transforms_for_objects([obj for obj in objects if obj is not None]) tf_time = time if time else self.getLatestCommonTime(source_frame, target_frame) translation, rotation = self.lookupTransform(source_frame, target_frame, tf_time) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 5f892a291..7e29720e9 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -254,7 +254,7 @@ def get_camera_frame(self) -> str: :return: A name of the link of a camera """ - return self.cameras[list(self.cameras.keys())[0]].link_name + return f"{self.name}/{self.cameras[list(self.cameras.keys())[0]].link_name}" def get_default_camera(self) -> CameraDescription: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5b9c9ceaa..8e74014eb 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,8 +3,10 @@ from time import sleep import numpy as np +from matplotlib import pyplot as plt +import matplotlib.colors as mcolors from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional +from typing_extensions import List, Dict, Optional, Union, Tuple from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI @@ -139,6 +141,124 @@ def _make_sure_multiverse_resources_are_added(self): World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True + def get_images_for_target(self, + target_pose: Pose, + cam_pose: Pose, + size: Optional[int] = 256, + camera_min_distance: float = 0.1, + camera_max_distance: int = 3, + plot: bool = False) -> List[np.ndarray]: + """ + Calculates the view and projection Matrix and returns 3 images: + + 1. An RGB image + 2. A depth image + 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object + + :param target_pose: The pose to which the camera should point. + :param cam_pose: The pose of the camera. + :param size: The height and width of the images in pixels. + :param camera_min_distance: The near distance of the camera. + :param camera_max_distance: The maximum distance that the shot rays should travel. + :param plot: Whether to plot the segmentation mask and the depth image. + :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. + """ + + # Make the start position start from the minimum distance of the camera relative to the camera frame + camera_description = RobotDescription.current_robot_description.get_default_camera() + camera_frame = RobotDescription.current_robot_description.get_camera_frame() + camera_pose_in_camera_frame = self.local_transformer.transform_pose(cam_pose, camera_frame) + start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance + + np.array(camera_pose_in_camera_frame.position_as_list())) + start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation, camera_frame) + start_pose = self.local_transformer.transform_pose(start_pose, "map") + + # construct the list of start positions of the rays + rays_start_positions = np.repeat(np.array([start_pose.position_as_list()]), size * size, axis=0).tolist() + + # get the camera description + camera_horizontal_fov = camera_description.horizontal_angle + camera_vertical_fov = camera_description.vertical_angle + + # construct a 2d grid of rays angles + rays_horizontal_angles = np.linspace(-camera_horizontal_fov / 2, camera_horizontal_fov / 2, size) + rays_horizontal_angles = np.tile(rays_horizontal_angles, (size, 1)) + rays_vertical_angles = np.linspace(-camera_vertical_fov / 2, camera_vertical_fov / 2, size) + rays_vertical_angles = np.tile(rays_vertical_angles, (size, 1)).T + + # construct a 2d grid of rays end positions + rays_end_positions_x = camera_max_distance * np.cos(rays_vertical_angles) * np.sin(rays_horizontal_angles) + rays_end_positions_x = rays_end_positions_x.reshape(-1) + rays_end_positions_z = camera_max_distance * np.cos(rays_vertical_angles) * np.cos(rays_horizontal_angles) + rays_end_positions_z = rays_end_positions_z.reshape(-1) + rays_end_positions_y = camera_max_distance * np.sin(rays_vertical_angles) + rays_end_positions_y = rays_end_positions_y.reshape(-1) + rays_end_positions = np.stack((rays_end_positions_x, rays_end_positions_y, rays_end_positions_z), axis=1) + + # transform the rays end positions from camera frame to world frame + cam_to_world_transform = cam_pose.to_transform(camera_frame).get_homogeneous_matrix() + # add the homogeneous coordinate, by adding a column of ones to the position vectors, becoming 4xN matrix + homogenous_end_positions = np.concatenate((rays_end_positions, np.ones((size * size, 1))), axis=1).T + rays_end_positions = cam_to_world_transform @ homogenous_end_positions + rays_end_positions = rays_end_positions[:3, :].T.tolist() + + # apply the ray test + object_ids, distances = self.ray_test_batch(rays_start_positions, rays_end_positions, return_distance=True) + segmentation_mask = np.array(object_ids).squeeze(axis=1).reshape(size, size) + depth_image = np.array(distances).reshape(size, size) + camera_min_distance + normalized_depth_image = (depth_image - camera_min_distance) * 255 / (camera_max_distance - camera_min_distance) + color_depth_image = np.repeat(normalized_depth_image[:, :, np.newaxis], 3, axis=2).astype(np.uint8) + if plot: + self.plot_segmentation_mask(segmentation_mask) + self.plot_depth_image(depth_image) + return [color_depth_image, depth_image, segmentation_mask] + + def plot_segmentation_mask(self, segmentation_mask): + # Create a custom color map + unique_ids = np.unique(segmentation_mask) + unique_ids = unique_ids[unique_ids != -1] # Exclude -1 values + + # Create a color map that assigns a unique color to each ID + colors = plt.cm.get_cmap('tab20', len(unique_ids)) # Use tab20 colormap for distinct colors + color_dict = {uid: colors(i) for i, uid in enumerate(unique_ids)} + + # Map each ID to its corresponding color + segmentation_colored = np.zeros((256, 256, 3)) + + for uid in unique_ids: + segmentation_colored[segmentation_mask == uid] = color_dict[uid][:3] # Ignore the alpha channel + + # Create a colormap for the color bar + cmap = mcolors.ListedColormap([color_dict[uid][:3] for uid in unique_ids]) + norm = mcolors.BoundaryNorm(boundaries=np.arange(len(unique_ids) + 1) - 0.5, ncolors=len(unique_ids)) + + # Plot the colored segmentation mask + fig, ax = plt.subplots() + cax = ax.imshow(segmentation_colored) + ax.axis('off') # Hide axes + ax.set_title('Segmentation Mask with Different Colors for Each Object') + + # Create color bar + cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), ax=ax, ticks=np.arange(len(unique_ids))) + cbar.ax.set_yticklabels([self.get_object_by_id(uid).name for uid in unique_ids]) # Label the color bar with object IDs + cbar.set_label('Object Name') + + plt.show() + + @staticmethod + def plot_depth_image(depth_image): + # Plot the depth image + fig, ax = plt.subplots() + cax = ax.imshow(depth_image, cmap='viridis', vmin=0, vmax=np.max(depth_image)) + ax.axis('off') # Hide axes + ax.set_title('Depth Image') + + # Create color bar + cbar = fig.colorbar(cax, ax=ax) + cbar.set_label('Depth Value') + + plt.show() + def remove_multiverse_resources(self): """ Remove the multiverse resources from the pycram world resources. @@ -507,13 +627,17 @@ def ray_test(self, from_position: List[float], to_position: List[float]) -> Opti ray_test_result = self.ray_test_batch([from_position], [to_position])[0] return ray_test_result[0] if ray_test_result[0] != -1 else None - def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], - num_threads: int = 1) -> List[List[int]]: + def ray_test_batch(self, from_positions: List[List[float]], + to_positions: List[List[float]], + num_threads: int = 1, + return_distance: bool = False) -> Union[List[List[int]], + Optional[Tuple[List[List[int]], List[float]]]]: """ Note: Currently, num_threads is not used in Multiverse. """ ray_results = self.api_requester.get_objects_intersected_with_rays(from_positions, to_positions) results = [] + distances = [] for ray_result in ray_results: results.append([]) if ray_result.intersected(): @@ -529,7 +653,12 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L break else: results[-1].append(-1) - return results + if return_distance: + distances.append(ray_result.distance) + if return_distance: + return results, distances + else: + return results def step(self): """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ad65aeeec..a927e0dd4 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -2,6 +2,7 @@ import os import unittest +import matplotlib.pyplot as plt import numpy as np import psutil from tf.transformations import quaternion_from_euler, quaternion_multiply @@ -48,8 +49,7 @@ class MultiversePyCRAMTestCase(unittest.TestCase): def setUpClass(cls): if not multiverse_installed: return - cls.multiverse = Multiverse(simulation="pycram_test", - is_prospection=False) + cls.multiverse = Multiverse(simulation="pycram_test") @classmethod def tearDownClass(cls): @@ -110,11 +110,30 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): LookAtAction(targets=[pick_pose]).resolve().perform() - # object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() - object_desig = BelieveObject(types=[obj_type]).resolve() + object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + # object_desig = BelieveObject(types=[obj_type]).resolve() return object_desig + def test_get_images_for_target(self): + robot = self.spawn_robot(robot_name='pr2') + camera_description = self.multiverse.robot_description.get_default_camera() + camera_link_name = camera_description.link_name + camera_pose = robot.get_link_pose(camera_link_name) + camera_frame = self.multiverse.robot_description.get_camera_frame() + camera_front_facing_axis = camera_description.front_facing_axis + milk_spawn_position = np.array(camera_front_facing_axis) * 0.5 + orientation = camera_pose.to_transform(camera_frame).invert().rotation_as_list() + milk = self.spawn_milk(milk_spawn_position.tolist(), orientation, frame=camera_frame) + _, depth, segmentation_mask = self.multiverse.get_images_for_target(milk.pose, camera_pose, plot=False) + self.assertIsInstance(depth, np.ndarray) + self.assertIsInstance(segmentation_mask, np.ndarray) + self.assertTrue(depth.shape == (256, 256)) + self.assertTrue(segmentation_mask.shape == (256, 256)) + self.assertAlmostEqual(np.max(depth), 0.5, delta=0.1) + self.assertTrue(np.unique(segmentation_mask).shape[0] == 2) + self.assertTrue(milk.id in np.unique(segmentation_mask).flatten().tolist()) + def test_reset_world(self): set_position = [1, 1, 0.1] milk = self.spawn_milk(set_position) @@ -354,11 +373,11 @@ def spawn_big_bowl() -> Object: return big_bowl @staticmethod - def spawn_milk(position: List, orientation: Optional[List] = None) -> Object: + def spawn_milk(position: List, orientation: Optional[List] = None, frame=None) -> Object: if orientation is None: orientation = [0, 0, 0, 1] milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", - pose=Pose(position, orientation)) + pose=Pose(position, orientation, frame=frame)) return milk def spawn_robot(self, position: Optional[List[float]] = None, From 44f9b61fe7b54fce5f9397b834ffb34426627073 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 7 Aug 2024 16:57:35 +0200 Subject: [PATCH 125/189] [MultiverseAction] Fixed camera frmae issue, use camera_link instead. --- src/pycram/local_transformer.py | 14 +++++++------- src/pycram/pose_generator_and_validator.py | 8 +++++--- src/pycram/process_modules/pr2_process_modules.py | 3 ++- src/pycram/robot_description.py | 8 ++++++++ test/test_bullet_world.py | 8 ++++---- test/test_bullet_world_reasoning.py | 8 ++++---- 6 files changed, 30 insertions(+), 19 deletions(-) diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 91f07b527..e2eafc39c 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -31,6 +31,7 @@ class LocalTransformer(TransformerROS): """ _instance = None + prospection_prefix: str = "prospection/" def __new__(cls, *args, **kwargs): if not cls._instance: @@ -109,24 +110,23 @@ def get_object_name_for_frame(self, frame: str) -> Optional[str]: :param frame: The frame for which the object name should be returned :return: The name of the object associated with the frame """ + world = self.prospection_world if self.prospection_prefix in frame else self.world if frame == "map": return None - obj_name = [obj.name for obj in self.world.objects if frame == obj.tf_frame] + obj_name = [obj.name for obj in world.objects if frame == obj.tf_frame] return obj_name[0] if len(obj_name) > 0 else self.get_object_name_for_link_frame(frame) - def get_object_name_for_link_frame(self, link_frame: str) -> str: + def get_object_name_for_link_frame(self, link_frame: str) -> Optional[str]: """ Returns the name of the object that is associated with the given link frame. :param link_frame: The frame of the link for which the object name should be returned :return: The name of the object associated with the link frame """ - object_name = [obj.name for obj in self.world.objects for link in obj.links.values() + world = self.prospection_world if self.prospection_prefix in link_frame else self.world + object_name = [obj.name for obj in world.objects for link in obj.links.values() if link_frame in (link.name, link.tf_frame)] - if len(object_name) > 0: - return object_name[0] - else: - raise ObjectFrameNotFoundError(link_frame) + return object_name[0] if len(object_name) > 0 else None def lookup_transform_from_source_to_target_frame(self, source_frame: str, target_frame: str, time: Optional[rospy.rostime.Time] = None) -> Transform: diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index bb19fa0ff..07286e279 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -120,13 +120,13 @@ def visibility_validator(pose: Pose, robot_pose = robot.get_pose() if isinstance(object_or_pose, Object): robot.set_pose(pose) - camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) ray = world.ray_test(camera_pose.position_as_list(), object_or_pose.get_position_as_list()) res = ray == object_or_pose.id else: robot.set_pose(pose) - camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + camera_pose = robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link()) robot.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) # TODO: Check if this is correct ray = world.ray_test(camera_pose.position_as_list(), object_or_pose) @@ -186,7 +186,9 @@ def reachability_validator(pose: Pose, res = False arms = [] for description in manipulator_descs: - retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(description.end_effector.tool_frame)) + retract_target_pose = LocalTransformer().transform_pose(target, + robot.get_link_tf_frame( + description.end_effector.tool_frame)) retract_target_pose.position.x -= 0.07 # Care hard coded value copied from PlaceAction class # retract_pose needs to be in world frame? diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 62112ec5e..9736d1222 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -94,8 +94,9 @@ def _execute(self, desig: DetectingMotion): front_facing_axis = camera_description.front_facing_axis objects = World.current_world.get_object_by_type(object_type) + camera_link_name = [link.name for link in robot.links.values() if link.tf_frame == cam_frame_name][0] for obj in objects: - if btr.visible(obj, robot.get_link_pose(cam_frame_name), front_facing_axis): + if btr.visible(obj, robot.get_link_pose(camera_link_name), front_facing_axis): return obj diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 7e29720e9..9ad707f82 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -248,6 +248,14 @@ def get_manipulator_chains(self) -> List[KinematicChainDescription]: result.append(chain) return result + def get_camera_link(self) -> str: + """ + Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. + + :return: A name of the link of a camera + """ + return self.cameras[list(self.cameras.keys())[0]].link_name + def get_camera_frame(self) -> str: """ Quick method to get the name of a link of a camera. Uses the first camera in the list of cameras. diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 05cf36cff..cd6990b5a 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -186,20 +186,20 @@ def test_prospection_object_position_does_not_change_with_real_object(self): self.world.remove_object(milk_2) def test_add_vis_axis(self): - self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame())) + self.world.add_vis_axis(self.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_link())) self.assertTrue(len(self.world.vis_axis) == 1) self.world.remove_vis_axis() self.assertTrue(len(self.world.vis_axis) == 0) def test_add_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id = self.world.add_text("test", link.position_as_list, link.orientation_as_list, 1, Color(1, 0, 0, 1), 3, link.object_id, link.id) if self.world.mode == WorldMode.GUI: time.sleep(4) def test_remove_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, @@ -212,7 +212,7 @@ def test_remove_text(self): time.sleep(3) def test_remove_all_text(self): - link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_frame()) + link: ObjectDescription.Link = self.robot.get_link(RobotDescription.current_robot_description.get_camera_link()) text_id_1 = self.world.add_text("test 1", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, Color(1, 0, 0, 1), 0, link.object_id, link.id) text_id = self.world.add_text("test 2", link.pose.position_as_list(), link.pose.orientation_as_list(), 1, diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index fe9998228..8d8c1061b 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -20,16 +20,16 @@ def test_visible(self): self.milk.set_pose(Pose([1.5, 0, 1.2])) self.robot.set_pose(Pose()) time.sleep(1) - camera_frame = RobotDescription.current_robot_description.get_camera_frame() - self.world.add_vis_axis(self.robot.get_link_pose(camera_frame)) - self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(camera_frame), + camera_link = RobotDescription.current_robot_description.get_camera_link() + self.world.add_vis_axis(self.robot.get_link_pose(camera_link)) + self.assertTrue(btr.visible(self.milk, self.robot.get_link_pose(camera_link), RobotDescription.current_robot_description.get_default_camera().front_facing_axis)) def test_occluding(self): self.milk.set_pose(Pose([3, 0, 1.2])) self.robot.set_pose(Pose()) self.assertTrue(btr.occluding(self.milk, self.robot.get_link_pose( - RobotDescription.current_robot_description.get_camera_frame()), + RobotDescription.current_robot_description.get_camera_link()), RobotDescription.current_robot_description.get_default_camera().front_facing_axis) != []) def test_reachable(self): From 4736b31dda54510147be02e84fdbc89759297fec Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 7 Aug 2024 17:40:38 +0200 Subject: [PATCH 126/189] [MultiverseAction] DetectionAction is now working in multiverse. --- src/pycram/process_modules/pr2_process_modules.py | 3 +-- src/pycram/world_concepts/world_object.py | 6 +++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 9736d1222..50e07adf5 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -87,14 +87,13 @@ def _execute(self, desig: DetectingMotion): robot = World.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" - cam_frame_name = RobotDescription.current_robot_description.get_camera_frame() + camera_link_name = RobotDescription.current_robot_description.get_camera_link() # should be [0, 0, 1] camera_description = RobotDescription.current_robot_description.cameras[ list(RobotDescription.current_robot_description.cameras.keys())[0]] front_facing_axis = camera_description.front_facing_axis objects = World.current_world.get_object_by_type(object_type) - camera_link_name = [link.name for link in robot.links.values() if link.tf_frame == cam_frame_name][0] for obj in objects: if btr.visible(obj, robot.get_link_pose(camera_link_name), front_facing_axis): return obj diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9a3c0b237..dde2bf08a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -814,7 +814,11 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: :param joint_states: A dictionary with the joint id as key and the current state of the joint as value. """ for joint in self.joints.values(): - joint.current_state = joint_states[joint.id] + if joint.name not in self.robot_virtual_move_base_joints_names(): + joint.current_state = joint_states[joint.id] + + def robot_virtual_move_base_joints_names(self): + return self.robot_description.virtual_move_base_joints.names def remove_saved_states(self) -> None: """ From 8ad2986027d8baca03d3ec3fb783ef9e57d4a0c2 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 7 Aug 2024 18:10:30 +0200 Subject: [PATCH 127/189] [MultiverseAction] Added multiverse demo. --- demos/pycram_multiverse_demo/demo.py | 44 +++++++++++++++++++++++ src/pycram/datastructures/world.py | 3 +- test/test_multiverse.py | 54 +++------------------------- 3 files changed, 50 insertions(+), 51 deletions(-) create mode 100644 demos/pycram_multiverse_demo/demo.py diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py new file mode 100644 index 000000000..814d08490 --- /dev/null +++ b/demos/pycram_multiverse_demo/demo.py @@ -0,0 +1,44 @@ +from pycram.datastructures.dataclasses import Color +from pycram.datastructures.enums import ObjectType, Arms +from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ + LookAtAction, DetectAction +from pycram.designators.object_designator import BelieveObject +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.world_concepts.world_object import Object +from pycram.worlds.multiverse import Multiverse + + +@with_simulated_robot +def move_and_detect(obj_type: ObjectType, pick_pose: Pose): + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[pick_pose]).resolve().perform() + + object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + + return object_desig + + +world = Multiverse(simulation='pycram_test') +extension = ObjectDescription.get_file_extension() +robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) +apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") + +milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), + color=Color(1, 0, 0, 1)) + +pick_pose = Pose([2.6, 2.15, 1]) + +with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + milk_desig = move_and_detect(ObjectType.MILK, pick_pose) + + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + +world.reset_world_and_remove_objects() +world.exit() diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index da5937212..dcd67afc7 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1453,7 +1453,8 @@ def run(self): """ while not self.terminate: self.sync_lock.acquire() - self.sync_worlds() + if not self.terminate: + self.sync_worlds() self.sync_lock.release() time.sleep(WorldSync.WAIT_TIME_AS_N_SIMULATION_STEPS * self.world.simulation_time_step) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index a927e0dd4..ad85182c0 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -2,23 +2,17 @@ import os import unittest -import matplotlib.pyplot as plt import numpy as np import psutil from tf.transformations import quaternion_from_euler, quaternion_multiply from typing_extensions import Optional, List -from pycram.datastructures.dataclasses import Color, ContactPointsList, ContactPoint -from pycram.datastructures.enums import ObjectType, Arms, JointType, Grasp +from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint +from pycram.datastructures.enums import ObjectType, Arms, JointType from pycram.datastructures.pose import Pose -from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, NavigateAction, LookAtAction, \ - DetectAction, TransportAction, PickUpAction -from pycram.designators.object_designator import BelieveObject -from pycram.object_descriptors.urdf import ObjectDescription from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions -from pycram.process_module import simulated_robot, with_simulated_robot from pycram.worlds.multiverse_extras.helpers import get_robot_mjcf_path, parse_mjcf_actuators multiverse_installed = True @@ -75,46 +69,6 @@ def test_get_actuator_for_joint(self): actuator_name = robot.get_actuator_for_joint(robot.joints[joint_name]) self.assertEqual(actuator_name, "arm_right_1_actuator") - def test_demo(self): - extension = ObjectDescription.get_file_extension() - robot = self.spawn_robot(robot_name='pr2', position=[1.3, 2, 0.01]) - apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") - - milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), - color=Color(1, 0, 0, 1)) - # bowl = Object("big_bowl", ObjectType.BOWL, f"BigBowl.obj", pose=Pose([2.5, 2.3, 1]), - # color=Color(1, 1, 0, 1)) - # spoon = Object("soup_spoon", ObjectType.SPOON, f"SoupSpoon.obj", pose=Pose([2.6, 2.6, 1]), - # color=Color(0, 0, 1, 1)) - # bowl.attach(spoon) - # bowl.set_position([2.5, 2.4, 1.02]) - - pick_pose = Pose([2.6, 2.15, 1]) - - robot_desig = BelieveObject(names=[robot.name]) - apartment_desig = BelieveObject(names=[apartment.name]) - - with simulated_robot: - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.25]).resolve().perform() - - milk_desig = self.move_and_detect(ObjectType.MILK, pick_pose) - - TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() - - @staticmethod - @with_simulated_robot - def move_and_detect(obj_type: ObjectType, pick_pose: Pose): - NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() - - LookAtAction(targets=[pick_pose]).resolve().perform() - - object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() - # object_desig = BelieveObject(types=[obj_type]).resolve() - - return object_desig - def test_get_images_for_target(self): robot = self.spawn_robot(robot_name='pr2') camera_description = self.multiverse.robot_description.get_default_camera() @@ -331,7 +285,7 @@ def test_get_object_contact_points(self): self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) cup = self.spawn_cup([1, 1, 0.1]) - self.multiverse.simulate(0.1) + self.multiverse.simulate(0.2) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -373,7 +327,7 @@ def spawn_big_bowl() -> Object: return big_bowl @staticmethod - def spawn_milk(position: List, orientation: Optional[List] = None, frame=None) -> Object: + def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") -> Object: if orientation is None: orientation = [0, 0, 0, 1] milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf", From ca28fd2deeb58fff9bbfe36f78b17e5539400805 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 13 Aug 2024 19:05:51 +0200 Subject: [PATCH 128/189] [MultiverseAction] Removed exceptions.py and put them in the plan_failures.py --- examples/cram_plan_tutorial.md | 2 +- examples/improving_actions.md | 2 +- examples/language.md | 2 +- examples/minimal_task_tree.md | 2 +- examples/orm_querying_examples.md | 15 +- src/pycram/datastructures/dataclasses.py | 130 ++++++++++++++---- src/pycram/datastructures/world.py | 2 +- src/pycram/description.py | 2 +- src/pycram/designators/action_designator.py | 7 +- src/pycram/designators/motion_designator.py | 2 +- .../probabilistic/probabilistic_action.py | 2 +- src/pycram/exceptions.py | 36 ----- src/pycram/external_interfaces/ik.py | 2 +- src/pycram/failure_handling.py | 2 +- src/pycram/{plan_failures.py => failures.py} | 44 ++++++ src/pycram/language.py | 4 +- src/pycram/local_transformer.py | 3 - src/pycram/pose_generator_and_validator.py | 3 +- src/pycram/tasktree.py | 2 +- src/pycram/world_concepts/world_object.py | 2 +- test/test_database_resolver.py | 2 +- test/test_failure_handling.py | 2 +- test/test_language.py | 2 +- test/test_move_and_pick_up.py | 2 +- test/test_task_tree.py | 4 +- 25 files changed, 182 insertions(+), 96 deletions(-) delete mode 100644 src/pycram/exceptions.py rename src/pycram/{plan_failures.py => failures.py} (86%) diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index 7606b9d3d..7ca106a34 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -28,7 +28,7 @@ from pycram.designators.location_designator import * from pycram.process_module import simulated_robot from pycram.designators.object_designator import * import anytree -import pycram.plan_failures +import pycram.failures ``` Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal. diff --git a/examples/improving_actions.md b/examples/improving_actions.md index 3e796ed74..913df8fd5 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -44,7 +44,7 @@ from random_events.product_algebra import Event, SimpleEvent import pycram.orm.base from pycram.designators.action_designator import MoveTorsoActionPerformable -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.designators.object_designator import ObjectDesignatorDescription from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object diff --git a/examples/language.md b/examples/language.md index 0f036e673..1e605904a 100644 --- a/examples/language.md +++ b/examples/language.md @@ -254,7 +254,7 @@ We will see how exceptions are handled at a simple example. from pycram.designators.action_designator import * from pycram.process_module import simulated_robot from pycram.language import Code -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure def code_test(): diff --git a/examples/minimal_task_tree.md b/examples/minimal_task_tree.md index b057fe463..5aefc0df1 100644 --- a/examples/minimal_task_tree.md +++ b/examples/minimal_task_tree.md @@ -31,7 +31,7 @@ from pycram.designators.object_designator import * from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, WorldMode import anytree -import pycram.plan_failures +import pycram.failures ``` Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal. diff --git a/examples/orm_querying_examples.md b/examples/orm_querying_examples.md index 610c14f99..37dd77077 100644 --- a/examples/orm_querying_examples.md +++ b/examples/orm_querying_examples.md @@ -20,7 +20,6 @@ In this tutorial, we will get to see more examples of ORM querying. First, we will gather a lot of data. In order to achieve that we will write a randomized experiment for grasping a couple of objects. In the experiment the robot will try to grasp a randomized object using random poses and torso heights. - ```python from tf import transformations import itertools @@ -36,9 +35,10 @@ import tqdm import pycram.orm.base from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object as BulletWorldObject -from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction, ParkArmsActionPerformable, MoveTorsoActionPerformable +from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction, + ParkArmsActionPerformable, MoveTorsoActionPerformable from pycram.designators.object_designator import ObjectDesignatorDescription -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.process_module import ProcessModule from pycram.datastructures.enums import Arms, ObjectType, Grasp @@ -81,10 +81,11 @@ class GraspingExplorer: self.robots: List[Tuple[str, str]] = [("pr2", "pr2.urdf")] if not objects: - self.objects: List[Tuple[str, ObjectType, str]] = [("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl"), - ("bowl", ObjectType.BOWL, "bowl.stl"), - ("milk", ObjectType.MILK, "milk.stl"), - ("spoon", ObjectType.SPOON, "spoon.stl")] + self.objects: List[Tuple[str, ObjectType, str]] = [ + ("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl"), + ("bowl", ObjectType.BOWL, "bowl.stl"), + ("milk", ObjectType.MILK, "milk.stl"), + ("spoon", ObjectType.SPOON, "spoon.stl")] if not arms: self.arms: List[str] = [Arms.LEFT, Arms.RIGHT] diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 5f393475f..f7dcdab28 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -18,7 +18,7 @@ def get_point_as_list(point: Point) -> List[float]: """ - Returns the point as a list. + Return the point as a list. :param point: The point. :return: The point as a list @@ -41,7 +41,7 @@ class Color: @classmethod def from_list(cls, color: List[float]): """ - Sets the rgba_color from a list of RGBA values. + Set the rgba_color from a list of RGBA values. :param color: The list of RGBA values """ @@ -55,7 +55,7 @@ def from_list(cls, color: List[float]): @classmethod def from_rgb(cls, rgb: List[float]): """ - Sets the rgba_color from a list of RGB values. + Set the rgba_color from a list of RGB values. :param rgb: The list of RGB values """ @@ -64,7 +64,7 @@ def from_rgb(cls, rgb: List[float]): @classmethod def from_rgba(cls, rgba: List[float]): """ - Sets the rgba_color from a list of RGBA values. + Set the rgba_color from a list of RGBA values. :param rgba: The list of RGBA values """ @@ -72,7 +72,7 @@ def from_rgba(cls, rgba: List[float]): def get_rgba(self) -> List[float]: """ - Returns the rgba_color as a list of RGBA values. + Return the rgba_color as a list of RGBA values. :return: The rgba_color as a list of RGBA values """ @@ -80,7 +80,7 @@ def get_rgba(self) -> List[float]: def get_rgb(self) -> List[float]: """ - Returns the rgba_color as a list of RGB values. + Return the rgba_color as a list of RGB values. :return: The rgba_color as a list of RGB values """ @@ -102,7 +102,7 @@ class AxisAlignedBoundingBox: @classmethod def from_min_max(cls, min_point: List[float], max_point: List[float]): """ - Sets the axis-aligned bounding box from a minimum and maximum point. + Set the axis-aligned bounding box from a minimum and maximum point. :param min_point: The minimum point :param max_point: The maximum point @@ -111,7 +111,7 @@ def from_min_max(cls, min_point: List[float], max_point: List[float]): def get_min_max_points(self) -> Tuple[Point, Point]: """ - Returns the axis-aligned bounding box as a tuple of minimum and maximum points. + Return the axis-aligned bounding box as a tuple of minimum and maximum points. :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ @@ -119,7 +119,7 @@ def get_min_max_points(self) -> Tuple[Point, Point]: def get_min_point(self) -> Point: """ - Returns the axis-aligned bounding box as a minimum point. + Return the axis-aligned bounding box as a minimum point. :return: The axis-aligned bounding box as a minimum point """ @@ -127,7 +127,7 @@ def get_min_point(self) -> Point: def get_max_point(self) -> Point: """ - Returns the axis-aligned bounding box as a maximum point. + Return the axis-aligned bounding box as a maximum point. :return: The axis-aligned bounding box as a maximum point """ @@ -135,7 +135,7 @@ def get_max_point(self) -> Point: def get_min_max(self) -> Tuple[List[float], List[float]]: """ - Returns the axis-aligned bounding box as a tuple of minimum and maximum points. + Return the axis-aligned bounding box as a tuple of minimum and maximum points. :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ @@ -160,12 +160,18 @@ def get_max(self) -> List[float]: @dataclass class CollisionCallbacks: + """ + Dataclass for storing the collision callbacks. + """ on_collision_cb: Callable no_collision_cb: Optional[Callable] = None @dataclass class MultiBody: + """ + Dataclass for storing the information of a multibody which consists of a base and multiple links with joints. + """ base_visual_shape_index: int base_pose: Pose link_visual_shape_indices: List[int] @@ -180,13 +186,16 @@ class MultiBody: @dataclass class VisualShape(ABC): + """ + Abstract dataclass for storing the information of a visual shape. + """ rgba_color: Color visual_frame_position: List[float] @abstractmethod def shape_data(self) -> Dict[str, Any]: """ - Returns the shape data of the visual shape (e.g. half extents for a box, radius for a sphere). + Returns the shape data of the visual shape (e.g. half extents for a box, radius for a sphere) as a dictionary. """ pass @@ -194,13 +203,16 @@ def shape_data(self) -> Dict[str, Any]: @abstractmethod def visual_geometry_type(self) -> Shape: """ - Returns the visual geometry type of the visual shape (e.g. box, sphere). + Returns the visual geometry type of the visual shape (e.g. box, sphere) as a Shape object. """ pass @dataclass class BoxVisualShape(VisualShape): + """ + Dataclass for storing the information of a box visual shape + """ half_extents: List[float] def shape_data(self) -> Dict[str, List[float]]: @@ -217,6 +229,9 @@ def size(self) -> List[float]: @dataclass class SphereVisualShape(VisualShape): + """ + Dataclass for storing the information of a sphere visual shape + """ radius: float def shape_data(self) -> Dict[str, float]: @@ -229,6 +244,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class CapsuleVisualShape(VisualShape): + """ + Dataclass for storing the information of a capsule visual shape + """ radius: float length: float @@ -242,6 +260,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class CylinderVisualShape(CapsuleVisualShape): + """ + Dataclass for storing the information of a cylinder visual shape + """ @property def visual_geometry_type(self) -> Shape: @@ -250,6 +271,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class MeshVisualShape(VisualShape): + """ + Dataclass for storing the information of a mesh visual shape + """ scale: List[float] file_name: str @@ -263,6 +287,9 @@ def visual_geometry_type(self) -> Shape: @dataclass class PlaneVisualShape(VisualShape): + """ + Dataclass for storing the information of a plane visual shape + """ normal: List[float] def shape_data(self) -> Dict[str, List[float]]: @@ -292,10 +319,20 @@ def __eq__(self, other: 'LinkState'): return self.all_constraints_exist(other) and self.all_constraints_are_equal(other) def all_constraints_exist(self, other: 'LinkState'): + """ + Check if all constraints exist in the other link state. + + :param other: The state of the other link. + """ return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) def all_constraints_are_equal(self, other: 'LinkState'): + """ + Check if all constraints are equal to the ones in the other link state. + + :param other: The state of the other link. + """ return all([cid == other_cid for cid, other_cid in zip(self.constraint_ids.values(), other.constraint_ids.values())]) @@ -331,13 +368,28 @@ def __eq__(self, other: 'ObjectState'): and self.joint_states == other.joint_states) def pose_is_almost_equal(self, other: 'ObjectState'): + """ + Check if the pose of the object is almost equal to the pose of another object. + + :param other: The state of the other object. + """ return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) def all_attachments_exist(self, other: 'ObjectState'): + """ + Check if all attachments exist in the other object state. + + :param other: The state of the other object. + """ return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) and len(self.attachments.keys()) == len(other.attachments.keys())) def all_attachments_are_equal(self, other: 'ObjectState'): + """ + Check if all attachments are equal to the ones in the other object state. + + :param other: The state of the other object. + """ return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) @@ -354,13 +406,28 @@ def __eq__(self, other: 'WorldState'): and self.all_objects_states_are_equal(other)) def simulator_state_is_equal(self, other: 'WorldState'): + """ + Check if the simulator state is equal to the simulator state of another world state. + + :param other: The state of the other world. + """ return self.simulator_state_id == other.simulator_state_id def all_objects_exist(self, other: 'WorldState'): + """ + Check if all objects exist in the other world state. + + :param other: The state of the other world. + """ return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) and len(self.object_states.keys()) == len(other.object_states.keys())) def all_objects_states_are_equal(self, other: 'WorldState'): + """ + Check if all object states are equal to the ones in the other world state. + + :param other: The state of the other world. + """ return all([obj_state == other_obj_state for obj_state, other_obj_state in zip(self.object_states.values(), other.object_states.values())]) @@ -421,7 +488,8 @@ def __getitem__(self, item) -> Union[ContactPoint, 'ContactPointsList']: def get_normals_of_object(self, obj: Object) -> List[List[float]]: """ - Gets the normals of the object. + Get the normals of the object. + :param obj: An instance of the Object class that represents the object. :return: A list of float vectors that represent the normals of the object. """ @@ -429,14 +497,16 @@ def get_normals_of_object(self, obj: Object) -> List[List[float]]: def get_normals(self) -> List[List[float]]: """ - Gets the normals of the points. + Get the normals of the points. + :return: A list of float vectors that represent the normals of the contact points. """ return [point.normal_on_b for point in self] def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: """ - Gets the links in contact of the object. + Get the links in contact of the object. + :param obj: An instance of the Object class that represents the object. :return: A list of Link instances that represent the links in contact of the object. """ @@ -444,15 +514,17 @@ def get_links_in_contact_of_object(self, obj: Object) -> List[Link]: def get_points_of_object(self, obj: Object) -> 'ContactPointsList': """ - Gets the points of the object. - :param obj: - :return: + Get the points of the object. + + :param obj: An instance of the Object class that represents the object that the points are related to. + :return: A ContactPointsList instance that represents the contact points of the object. """ return ContactPointsList([point for point in self if point.link_b.object == obj]) def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]: """ - Returns the object that is not in the current points list but was in the initial points list. + Return the object that is not in the current points list but was in the initial points list. + :param previous_points: The initial points list. :return: A list of Object instances that represent the objects that got removed. """ @@ -462,7 +534,8 @@ def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: """ - Returns the object that is not in the initial points list but is in the current points list. + Return the object that is not in the initial points list but is in the current points list. + :param previous_points: The initial points list. :return: A list of Object instances that represent the new objects. """ @@ -472,16 +545,27 @@ def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]: def is_object_in_the_list(self, obj: Object) -> bool: """ - Checks if the object is one of the objects that have points in the list. + Check if the object is one of the objects that have points in the list. + :param obj: An instance of the Object class that represents the object. :return: True if the object is in the list, False otherwise. """ return obj in self.get_objects_that_have_points() def get_objects_that_have_points(self) -> List[Object]: + """ + Return the objects that have points in the list. + + :return: A list of Object instances that represent the objects that have points in the list. + """ return [point.link_b.object for point in self] def get_names_of_objects_that_have_points(self) -> List[str]: + """ + Return the names of the objects that have points in the list. + + :return: A list of strings that represent the names of the objects that have points in the list. + """ return [point.link_b.object.name for point in self] def __str__(self): @@ -520,7 +604,7 @@ class VirtualMoveBaseJoints: @property def names(self) -> List[str]: """ - Returns the names of the virtual move base joints. + Return the names of the virtual move base joints. """ return [self.translation_x, self.translation_y, self.angular_z] diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index dcd67afc7..e4493ed6b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -23,7 +23,7 @@ from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform from ..datastructures.world_entity import StateEntity -from ..exceptions import ProspectionObjectNotFound, WorldObjectNotFound +from ..failures import ProspectionObjectNotFound, WorldObjectNotFound from ..local_transformer import LocalTransformer from ..robot_description import RobotDescription from ..validation.goal_validator import (MultiPoseGoalValidator, diff --git a/src/pycram/description.py b/src/pycram/description.py index 2d2517066..06dcdd491 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -12,7 +12,7 @@ from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity -from .exceptions import ObjectDescriptionNotFound +from .failures import ObjectDescriptionNotFound from .local_transformer import LocalTransformer if TYPE_CHECKING: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index efdd8e59a..a6c734604 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -8,17 +8,14 @@ import numpy as np from sqlalchemy.orm import Session from tf import transformations -from typing_extensions import Any, List, Union, Callable, Optional, Type - -import rospy +from typing_extensions import List, Union, Callable, Optional, Type from .location_designator import CostmapLocation from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart from ..local_transformer import LocalTransformer -from ..plan_failures import ObjectUnfetchable, ReachabilityFailure -# from ..robot_descriptions import robot_description +from ..failures import ObjectUnfetchable, ReachabilityFailure from ..robot_description import RobotDescription from ..tasktree import with_tree diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index b008c8561..b67fe451a 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -5,7 +5,7 @@ from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from ..designator import ResolutionError from ..orm.base import ProcessMetaData -from ..plan_failures import PerceptionObjectNotFound +from ..failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, diff --git a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py index fe6668bee..4789a045c 100644 --- a/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py +++ b/src/pycram/designators/specialized_designators/probabilistic/probabilistic_action.py @@ -19,7 +19,7 @@ from ....designator import ActionDesignatorDescription, ObjectDesignatorDescription from ....local_transformer import LocalTransformer from ....orm.views import PickUpWithContextView -from ....plan_failures import ObjectUnreachable, PlanFailure +from ....failures import ObjectUnreachable, PlanFailure class Grasp(SetElement): diff --git a/src/pycram/exceptions.py b/src/pycram/exceptions.py deleted file mode 100644 index 51e6b2097..000000000 --- a/src/pycram/exceptions.py +++ /dev/null @@ -1,36 +0,0 @@ -from typing_extensions import TYPE_CHECKING - -if TYPE_CHECKING: - from pycram.world_concepts.world_object import Object - - -class ProspectionObjectNotFound(KeyError): - def __init__(self, obj: 'Object'): - super().__init__(f"The given object {obj.name} is not in the prospection world.") - - -class WorldObjectNotFound(KeyError): - def __init__(self, obj: 'Object'): - super().__init__(f"The given object {obj.name} is not in the main world.") - - -class ObjectAlreadyExists(Exception): - def __init__(self, obj: 'Object'): - super().__init__(f"An object with the name {obj.name} already exists in the world.") - - -class ObjectDescriptionNotFound(KeyError): - def __init__(self, object_name: str, path: str, extension: str): - super().__init__(f"{object_name} with path {path} and extension {extension} is not in supported extensions, and" - f" the description data was not found on the ROS parameter server") - - -class WorldMismatchErrorBetweenObjects(Exception): - def __init__(self, obj_1: 'Object', obj_2: 'Object'): - super().__init__(f"World mismatch between the attached objects {obj_1.name} and {obj_2.name}," - f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") - - -class ObjectFrameNotFoundError(KeyError): - def __init__(self, frame_name: str): - super().__init__(f"Frame {frame_name} does not belong to any of the objects in the world.") \ No newline at end of file diff --git a/src/pycram/external_interfaces/ik.py b/src/pycram/external_interfaces/ik.py index 8f59db07a..83f084ac6 100644 --- a/src/pycram/external_interfaces/ik.py +++ b/src/pycram/external_interfaces/ik.py @@ -14,7 +14,7 @@ from ..local_transformer import LocalTransformer from ..datastructures.pose import Pose from ..robot_description import RobotDescription -from ..plan_failures import IKError +from ..failures import IKError from ..external_interfaces.giskard import projection_cartesian_goal, allow_gripper_collision diff --git a/src/pycram/failure_handling.py b/src/pycram/failure_handling.py index 8fb266282..23fe6a088 100644 --- a/src/pycram/failure_handling.py +++ b/src/pycram/failure_handling.py @@ -1,5 +1,5 @@ from .designator import DesignatorDescription -from .plan_failures import PlanFailure +from .failures import PlanFailure class FailureHandling: diff --git a/src/pycram/plan_failures.py b/src/pycram/failures.py similarity index 86% rename from src/pycram/plan_failures.py rename to src/pycram/failures.py index e8ac32fe1..36b39febf 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/failures.py @@ -1,3 +1,9 @@ +from typing_extensions import TYPE_CHECKING + +if TYPE_CHECKING: + from pycram.world_concepts.world_object import Object + + class PlanFailure(Exception): """Implementation of plan failures.""" @@ -395,3 +401,41 @@ def __init__(*args, **kwargs): class CollisionError(PlanFailure): def __init__(*args, **kwargs): super().__init__(*args, **kwargs) + + +""" +The following exceptions are used in the PyCRAM framework to handle errors related to the world and the objects in it. +They are usually related to a bug in the code or a misuse of the framework (e.g. logical errors in the code). +""" + + +class ProspectionObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the prospection world.") + + +class WorldObjectNotFound(KeyError): + def __init__(self, obj: 'Object'): + super().__init__(f"The given object {obj.name} is not in the main world.") + + +class ObjectAlreadyExists(Exception): + def __init__(self, obj: 'Object'): + super().__init__(f"An object with the name {obj.name} already exists in the world.") + + +class ObjectDescriptionNotFound(KeyError): + def __init__(self, object_name: str, path: str, extension: str): + super().__init__(f"{object_name} with path {path} and extension {extension} is not in supported extensions, and" + f" the description data was not found on the ROS parameter server") + + +class WorldMismatchErrorBetweenObjects(Exception): + def __init__(self, obj_1: 'Object', obj_2: 'Object'): + super().__init__(f"World mismatch between the attached objects {obj_1.name} and {obj_2.name}," + f"obj_1.world: {obj_1.world}, obj_2.world: {obj_2.world}") + + +class ObjectFrameNotFoundError(KeyError): + def __init__(self, frame_name: str): + super().__init__(f"Frame {frame_name} does not belong to any of the objects in the world.") diff --git a/src/pycram/language.py b/src/pycram/language.py index 81612a78b..9ac7d2624 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -5,11 +5,11 @@ from typing_extensions import Iterable, Optional, Callable, Dict, Any, List, Union from anytree import NodeMixin, Node, PreOrderIter -from pycram.datastructures.enums import State +from .datastructures.enums import State import threading from .fluent import Fluent -from .plan_failures import PlanFailure, NotALanguageExpression +from .failures import PlanFailure, NotALanguageExpression from .external_interfaces import giskard diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index e2eafc39c..9cd58ea5e 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -1,8 +1,6 @@ import sys import logging -from .exceptions import ObjectFrameNotFoundError - if 'world' in sys.modules: logging.warning("(publisher) Make sure that you are not loading this module from pycram.world.") import rospy @@ -10,7 +8,6 @@ from tf import TransformerROS from rospy import Duration -from geometry_msgs.msg import TransformStamped from .datastructures.pose import Pose, Transform from typing_extensions import List, Optional, Union, Iterable diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py index 07286e279..ecebcdd51 100644 --- a/src/pycram/pose_generator_and_validator.py +++ b/src/pycram/pose_generator_and_validator.py @@ -9,8 +9,7 @@ from .datastructures.pose import Pose, Transform from .robot_description import RobotDescription from .external_interfaces.ik import request_ik -from .plan_failures import IKError -from .utils import _apply_ik +from .failures import IKError from typing_extensions import Tuple, List, Union, Dict, Iterable diff --git a/src/pycram/tasktree.py b/src/pycram/tasktree.py index 448e1f718..6ad6b32b2 100644 --- a/src/pycram/tasktree.py +++ b/src/pycram/tasktree.py @@ -17,7 +17,7 @@ from .datastructures.world import World from .orm.tasktree import TaskTreeNode as ORMTaskTreeNode from .orm.base import ProcessMetaData -from .plan_failures import PlanFailure +from .failures import PlanFailure from .datastructures.enums import TaskStatus from .datastructures.dataclasses import Color diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index dde2bf08a..a2f201bd8 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -16,7 +16,7 @@ from ..datastructures.world import World from ..datastructures.world_entity import WorldEntity from ..description import ObjectDescription, LinkDescription, Joint -from ..exceptions import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects +from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDFObject from ..robot_description import RobotDescriptionManager, RobotDescription diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 5f015d831..2a4ef53b4 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -2,7 +2,7 @@ import unittest import sqlalchemy import sqlalchemy.orm -import pycram.plan_failures +import pycram.failures from pycram.world_concepts.world_object import Object from pycram.datastructures.world import World from pycram.designators import action_designator diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index b28420f96..caef9dad4 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -7,7 +7,7 @@ from pycram.designators.action_designator import ParkArmsAction from pycram.datastructures.enums import ObjectType, Arms, WorldMode from pycram.failure_handling import Retry -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.process_module import ProcessModule, simulated_robot from pycram.robot_description import RobotDescription from pycram.object_descriptors.urdf import ObjectDescription diff --git a/test/test_language.py b/test/test_language.py index bb3fec509..fda13d6b4 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -5,7 +5,7 @@ from pycram.designators.object_designator import BelieveObject from pycram.datastructures.enums import ObjectType, State from pycram.fluent import Fluent -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.datastructures.pose import Pose from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Code from pycram.process_module import simulated_robot diff --git a/test/test_move_and_pick_up.py b/test/test_move_and_pick_up.py index 013a8708d..2c4268950 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_move_and_pick_up.py @@ -9,7 +9,7 @@ from pycram.designators.action_designator import MoveTorsoActionPerformable from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPickUp, GaussianCostmapModel) -from pycram.plan_failures import PlanFailure +from pycram.failures import PlanFailure from pycram.process_module import simulated_robot diff --git a/test/test_task_tree.py b/test/test_task_tree.py index dee20a698..6d1c43fb8 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -8,7 +8,7 @@ import unittest import anytree from bullet_world_testcase import BulletWorldTestCase -import pycram.plan_failures +import pycram.failures from pycram.designators import object_designator, action_designator @@ -52,7 +52,7 @@ def failing_plan(): pycram.tasktree.reset_tree() - self.assertRaises(pycram.plan_failures.PlanFailure, failing_plan) + self.assertRaises(pycram.failures.PlanFailure, failing_plan) tt = pycram.tasktree.task_tree From 84179554f3b91b63e306e075995ef4446e7c166c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 17:34:42 +0200 Subject: [PATCH 129/189] [MultiverseAction] Created RayTestUtils class in the utils.py and used it in multiverse.py. Added an apply transform to array of points in the Transform class. --- src/pycram/datastructures/pose.py | 13 ++ src/pycram/failures.py | 8 +- src/pycram/object_descriptors/urdf.py | 18 +- src/pycram/utils.py | 253 +++++++++++++++++++++++++- src/pycram/worlds/multiverse.py | 143 +++------------ test/test_task_tree.py | 2 +- 6 files changed, 303 insertions(+), 134 deletions(-) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index ea5860e2b..a8feade26 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -345,6 +345,19 @@ def __init__(self, translation: Optional[List[float]] = None, rotation: Optional self.frame = frame + def apply_transform_to_array_of_points(self, points: np.ndarray) -> np.ndarray: + """ + Applies this Transform to an array of points. The points are given as a Nx3 matrix, where N is the number of + points. The points are transformed from the child_frame_id to the frame_id of this Transform. + + :param points: The points that should be transformed, given as a Nx3 matrix. + """ + homogeneous_transform = self.get_homogeneous_matrix() + # add the homogeneous coordinate, by adding a column of ones to the position vectors, becoming 4xN matrix + homogenous_points = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1).T + rays_end_positions = homogeneous_transform @ homogenous_points + return rays_end_positions[:3, :].T + def get_homogeneous_matrix(self) -> np.ndarray: """ Returns the homogeneous matrix of this Transform. The matrix can be used to transform points from the frame_id diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 36b39febf..c28e5ed3b 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -1,4 +1,4 @@ -from typing_extensions import TYPE_CHECKING +from typing_extensions import TYPE_CHECKING, List if TYPE_CHECKING: from pycram.world_concepts.world_object import Object @@ -439,3 +439,9 @@ def __init__(self, obj_1: 'Object', obj_2: 'Object'): class ObjectFrameNotFoundError(KeyError): def __init__(self, frame_name: str): super().__init__(f"Frame {frame_name} does not belong to any of the objects in the world.") + + +class MultiplePossibleTipLinks(Exception): + def __init__(self, object_name: str, start_link: str, tip_links: List[str]): + super().__init__(f"Multiple possible tip links found for object {object_name} with start link {start_link}:" + f" {tip_links}") diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 4db3d32aa..6febc4468 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -17,6 +17,7 @@ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ SphereVisualShape, MeshVisualShape +from ..failures import MultiplePossibleTipLinks from ..utils import suppress_stdout_stderr @@ -317,12 +318,16 @@ def get_root(self) -> str: return self.parsed_description.get_root() def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + :raises MultiplePossibleTipLinks: If there are multiple possible tip links. + """ link = self.get_root() while link in self.parsed_description.child_map: children = self.parsed_description.child_map[link] if len(children) > 1: # Multiple children, can't decide which one to take (e.g. fingers of a hand) - break + raise MultiplePossibleTipLinks(self.parsed_description.name, link, [child[1] for child in children]) else: child = children[0][1] link = child @@ -363,6 +368,13 @@ def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: @staticmethod def make_mesh_paths_absolute(urdf_string: str, urdf_file_path: str) -> str: + """ + Convert all relative mesh paths in the URDF to absolute paths. + + :param urdf_string: The URDF description as string + :param urdf_file_path: The path to the URDF file + :returns: The new URDF description as string. + """ # Parse the URDF file root = ET.fromstring(urdf_string) @@ -413,7 +425,7 @@ def fix_missing_inertial(urdf_string: str) -> str: @staticmethod def remove_error_tags(urdf_string: str) -> str: """ - Removes all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + Remove all tags in the removing_tags list from the URDF since these tags are known to cause errors with the URDF_parser :param urdf_string: String of the URDF from which the tags should be removed @@ -431,7 +443,7 @@ def remove_error_tags(urdf_string: str) -> str: @staticmethod def fix_link_attributes(urdf_string: str) -> str: """ - Removes the attribute 'type' from links since this is not parsable by the URDF parser. + Remove the attribute 'type' from links since this is not parsable by the URDF parser. :param urdf_string: The string of the URDF from which the attributes should be removed :return: The URDF string with the attributes removed diff --git a/src/pycram/utils.py b/src/pycram/utils.py index c4cb7710e..9397b3df6 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -7,14 +7,20 @@ GeneratorList -- implementation of generator list wrappers. """ from inspect import isgeneratorfunction -from typing_extensions import List, Tuple, Callable - import os +import math + +import numpy as np +from matplotlib import pyplot as plt +import matplotlib.colors as mcolors +from typing_extensions import Tuple, Callable, List, Dict, TYPE_CHECKING from .datastructures.pose import Pose -import math +from .local_transformer import LocalTransformer -from typing_extensions import Dict +if TYPE_CHECKING: + from .world_concepts.world_object import Object + from .robot_description import CameraDescription class bcolors: @@ -36,7 +42,7 @@ class bcolors: UNDERLINE = '\033[4m' -def _apply_ik(robot: 'pycram.world_concepts.WorldObject', pose_and_joint_poses: Tuple[Pose, Dict[str, float]]) -> None: +def _apply_ik(robot: 'Object', pose_and_joint_poses: Tuple[Pose, Dict[str, float]]) -> None: """ Apllies a list of joint poses calculated by an inverse kinematics solver to a robot @@ -113,7 +119,7 @@ def axis_angle_to_quaternion(axis: List, angle: float) -> Tuple: z = normalized_axis[2] * math.sin(angle / 2) w = math.cos(angle / 2) - return (x, y, z, w) + return tuple((x, y, z, w)) class suppress_stdout_stderr(object): @@ -130,7 +136,7 @@ class suppress_stdout_stderr(object): def __init__(self): # Open a pair of null files - self.null_fds = [os.open(os.devnull, os.O_RDWR) for x in range(2)] + self.null_fds = [os.open(os.devnull, os.O_RDWR) for _ in range(2)] # Save the actual stdout (1) and stderr (2) file descriptors. self.save_fds = [os.dup(1), os.dup(2)] @@ -148,3 +154,236 @@ def __exit__(self, *_): # Close all file descriptors for fd in self.null_fds + self.save_fds: os.close(fd) + + +class RayTestUtils: + + def __init__(self, ray_test_batch: Callable, object_id_to_name: Dict = None): + """ + Initialize the ray test helper. + """ + self.local_transformer = LocalTransformer() + self.ray_test_batch = ray_test_batch + self.object_id_to_name = object_id_to_name + + def get_images_for_target(self, cam_pose: Pose, + camera_description: 'CameraDescription', + camera_frame: str, + size: int = 256, + camera_min_distance: float = 0.1, + camera_max_distance: int = 3, + plot: bool = False) -> List[np.ndarray]: + """ + Note: The returned color image is a repeated depth image in 3 channels. + """ + + # get the list of start positions of the rays. + rays_start_positions = self.get_camera_rays_start_positions(camera_description, camera_frame, cam_pose, size, + camera_min_distance).tolist() + + # get the list of end positions of the rays + rays_end_positions = self.get_camera_rays_end_positions(camera_description, camera_frame, cam_pose, size, + camera_max_distance).tolist() + + # apply the ray test + object_ids, distances = self.ray_test_batch(rays_start_positions, rays_end_positions, return_distance=True) + + # construct the images/masks + segmentation_mask = self.construct_segmentation_mask_from_ray_test_object_ids(object_ids, size) + depth_image = self.construct_depth_image_from_ray_test_distances(distances, size) + camera_min_distance + color_depth_image = self.construct_color_image_from_depth_image(depth_image) + + if plot: + self.plot_segmentation_mask(segmentation_mask) + self.plot_depth_image(depth_image) + + return [color_depth_image, depth_image, segmentation_mask] + + @staticmethod + def construct_segmentation_mask_from_ray_test_object_ids(object_ids: List[int], size: int) -> np.ndarray: + """ + Construct a segmentation mask from the object ids returned by the ray test. + + :param object_ids: The object ids. + :param size: The size of the grid. + :return: The segmentation mask. + """ + return np.array(object_ids).squeeze(axis=1).reshape(size, size) + + @staticmethod + def construct_depth_image_from_ray_test_distances(distances: List[float], size: int) -> np.ndarray: + """ + Construct a depth image from the distances returned by the ray test. + + :param distances: The distances. + :param size: The size of the grid. + :return: The depth image. + """ + return np.array(distances).reshape(size, size) + + @staticmethod + def construct_color_image_from_depth_image(depth_image: np.ndarray) -> np.ndarray: + """ + Construct a color image from the depth image. + + :param depth_image: The depth image. + :return: The color image. + """ + min_distance = np.min(depth_image) + max_distance = np.max(depth_image) + normalized_depth_image = (depth_image - min_distance) * 255 / (max_distance - min_distance) + return np.repeat(normalized_depth_image[:, :, np.newaxis], 3, axis=2).astype(np.uint8) + + def get_camera_rays_start_positions(self, camera_description: 'CameraDescription', camera_frame: str, + camera_pose: Pose, size: int, + camera_min_distance: float) -> np.ndarray: + + # get the start pose of the rays from the camera pose and minimum distance. + start_pose = self.get_camera_rays_start_pose(camera_description, camera_frame, camera_pose, camera_min_distance) + + # get the list of start positions of the rays. + return np.repeat(np.array([start_pose.position_as_list()]), size * size, axis=0) + + def get_camera_rays_start_pose(self, camera_description: 'CameraDescription', camera_frame: str, camera_pose: Pose, + camera_min_distance: float) -> Pose: + """ + Get the start position of the camera rays, which is the camera pose shifted by the minimum distance of the + camera. + + :param camera_description: The camera description. + :param camera_frame: The camera tf frame. + :param camera_pose: The camera pose. + :param camera_min_distance: The minimum distance from which the camera can see. + """ + camera_pose_in_camera_frame = self.local_transformer.transform_pose(camera_pose, camera_frame) + start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance + + np.array(camera_pose_in_camera_frame.position_as_list())) + start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation_as_list(), camera_frame) + return self.local_transformer.transform_pose(start_pose, "map") + + def get_camera_rays_end_positions(self, camera_description: 'CameraDescription', camera_frame: str, + camera_pose: Pose, size: int, camera_max_distance: float = 3.0) -> np.ndarray: + """ + Get the end positions of the camera rays. + + :param camera_description: The camera description. + :param camera_frame: The camera frame. + :param camera_pose: The camera pose. + :param size: The size of the grid. + :param camera_max_distance: The maximum distance of the camera. + :return: The end positions of the camera rays. + """ + rays_horizontal_angles, rays_vertical_angles = self.construct_grid_of_camera_rays_angles(camera_description, + size) + rays_end_positions = self.get_end_positions_of_rays_from_angles_and_distance(rays_vertical_angles, + rays_horizontal_angles, + camera_max_distance) + return self.transform_points_from_camera_frame_to_world_frame(camera_pose, camera_frame, rays_end_positions) + + @staticmethod + def transform_points_from_camera_frame_to_world_frame(camera_pose: Pose, camera_frame: str, + points: np.ndarray) -> np.ndarray: + """ + Transform points from the camera frame to the world frame. + + :param camera_pose: The camera pose. + :param camera_frame: The camera frame. + :param points: The points to transform. + :return: The transformed points. + """ + cam_to_world_transform = camera_pose.to_transform(camera_frame) + return cam_to_world_transform.apply_transform_to_array_of_points(points) + + @staticmethod + def get_end_positions_of_rays_from_angles_and_distance(vertical_angles: np.ndarray, horizontal_angles: np.ndarray, + distance: float) -> np.ndarray: + """ + Get the end positions of the rays from the angles and the distance. + + :param vertical_angles: The vertical angles of the rays. + :param horizontal_angles: The horizontal angles of the rays. + :param distance: The distance of the rays. + :return: The end positions of the rays. + """ + rays_end_positions_x = distance * np.cos(vertical_angles) * np.sin(horizontal_angles) + rays_end_positions_x = rays_end_positions_x.reshape(-1) + rays_end_positions_z = distance * np.cos(vertical_angles) * np.cos(horizontal_angles) + rays_end_positions_z = rays_end_positions_z.reshape(-1) + rays_end_positions_y = distance * np.sin(vertical_angles) + rays_end_positions_y = rays_end_positions_y.reshape(-1) + return np.stack((rays_end_positions_x, rays_end_positions_y, rays_end_positions_z), axis=1) + + @staticmethod + def construct_grid_of_camera_rays_angles(camera_description: 'CameraDescription', + size: int) -> Tuple[np.ndarray, np.ndarray]: + """ + Construct a 2D grid of camera rays angles. + + :param camera_description: The camera description. + :param size: The size of the grid. + :return: The 2D grid of the horizontal and the vertical angles of the camera rays. + """ + # get the camera fov angles + camera_horizontal_fov = camera_description.horizontal_angle + camera_vertical_fov = camera_description.vertical_angle + + # construct a 2d grid of rays angles + rays_horizontal_angles = np.linspace(-camera_horizontal_fov / 2, camera_horizontal_fov / 2, size) + rays_horizontal_angles = np.tile(rays_horizontal_angles, (size, 1)) + rays_vertical_angles = np.linspace(-camera_vertical_fov / 2, camera_vertical_fov / 2, size) + rays_vertical_angles = np.tile(rays_vertical_angles, (size, 1)).T + return rays_horizontal_angles, rays_vertical_angles + + def plot_segmentation_mask(self, segmentation_mask): + """ + Plot the segmentation mask with different colors for each object. + + :param segmentation_mask: The segmentation mask. + """ + + # Create a custom color map + unique_ids = np.unique(segmentation_mask) + unique_ids = unique_ids[unique_ids != -1] # Exclude -1 values + + # Create a color map that assigns a unique color to each ID + colors = plt.cm.get_cmap('tab20', len(unique_ids)) # Use tab20 colormap for distinct colors + color_dict = {uid: colors(i) for i, uid in enumerate(unique_ids)} + + # Map each ID to its corresponding color + mask_shape = segmentation_mask.shape + segmentation_colored = np.zeros((mask_shape[0], mask_shape[1], 3)) + + for uid in unique_ids: + segmentation_colored[segmentation_mask == uid] = color_dict[uid][:3] # Ignore the alpha channel + + # Create a colormap for the color bar + cmap = mcolors.ListedColormap([color_dict[uid][:3] for uid in unique_ids]) + norm = mcolors.BoundaryNorm(boundaries=np.arange(len(unique_ids) + 1) - 0.5, ncolors=len(unique_ids)) + + # Plot the colored segmentation mask + fig, ax = plt.subplots() + _ = ax.imshow(segmentation_colored) + ax.axis('off') # Hide axes + ax.set_title('Segmentation Mask with Different Colors for Each Object') + + # Create color bar + cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), ax=ax, ticks=np.arange(len(unique_ids))) + cbar.ax.set_yticklabels( + [self.object_id_to_name[uid] for uid in unique_ids]) # Label the color bar with object IDs + cbar.set_label('Object Name') + + plt.show() + + @staticmethod + def plot_depth_image(depth_image): + # Plot the depth image + fig, ax = plt.subplots() + cax = ax.imshow(depth_image, cmap='viridis', vmin=0, vmax=np.max(depth_image)) + ax.axis('off') # Hide axes + ax.set_title('Depth Image') + + # Create color bar + cbar = fig.colorbar(cax, ax=ax) + cbar.set_label('Depth Value') + + plt.show() diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 8e74014eb..6e84c57ae 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,10 +3,8 @@ from time import sleep import numpy as np -from matplotlib import pyplot as plt -import matplotlib.colors as mcolors from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple +from typing_extensions import List, Dict, Optional, Union, Tuple, Callable from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI @@ -24,6 +22,7 @@ validate_joint_position from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object +from ..utils import RayTestUtils class Multiverse(World): @@ -101,6 +100,8 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._init_constraint_and_object_id_name_map_collections() + self.ray_test_utils = RayTestUtils(self.ray_test_batch, self.object_id_to_name) + if not self.is_prospection_world: self._spawn_floor() @@ -108,6 +109,11 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.api_requester.pause_simulation() def _init_clients(self): + """ + Initialize the Multiverse clients that will be used to communicate with the Multiverse server. + Each client is responsible for a specific task, e.g. reading data from the server, writing data to the serve, + calling the API, or controlling the robot joints. + """ self.reader: MultiverseReader = self.client_manager.create_reader( is_prospection_world=self.is_prospection_world) self.writer: MultiverseWriter = self.client_manager.create_writer( @@ -141,124 +147,6 @@ def _make_sure_multiverse_resources_are_added(self): World.cache_manager.data_directory = World.data_directory self.added_multiverse_resources = True - def get_images_for_target(self, - target_pose: Pose, - cam_pose: Pose, - size: Optional[int] = 256, - camera_min_distance: float = 0.1, - camera_max_distance: int = 3, - plot: bool = False) -> List[np.ndarray]: - """ - Calculates the view and projection Matrix and returns 3 images: - - 1. An RGB image - 2. A depth image - 3. A segmentation Mask, the segmentation mask indicates for every pixel the visible Object - - :param target_pose: The pose to which the camera should point. - :param cam_pose: The pose of the camera. - :param size: The height and width of the images in pixels. - :param camera_min_distance: The near distance of the camera. - :param camera_max_distance: The maximum distance that the shot rays should travel. - :param plot: Whether to plot the segmentation mask and the depth image. - :return: A list containing an RGB and depth image as well as a segmentation mask, in this order. - """ - - # Make the start position start from the minimum distance of the camera relative to the camera frame - camera_description = RobotDescription.current_robot_description.get_default_camera() - camera_frame = RobotDescription.current_robot_description.get_camera_frame() - camera_pose_in_camera_frame = self.local_transformer.transform_pose(cam_pose, camera_frame) - start_position = (np.array(camera_description.front_facing_axis) * camera_min_distance - + np.array(camera_pose_in_camera_frame.position_as_list())) - start_pose = Pose(start_position.tolist(), camera_pose_in_camera_frame.orientation, camera_frame) - start_pose = self.local_transformer.transform_pose(start_pose, "map") - - # construct the list of start positions of the rays - rays_start_positions = np.repeat(np.array([start_pose.position_as_list()]), size * size, axis=0).tolist() - - # get the camera description - camera_horizontal_fov = camera_description.horizontal_angle - camera_vertical_fov = camera_description.vertical_angle - - # construct a 2d grid of rays angles - rays_horizontal_angles = np.linspace(-camera_horizontal_fov / 2, camera_horizontal_fov / 2, size) - rays_horizontal_angles = np.tile(rays_horizontal_angles, (size, 1)) - rays_vertical_angles = np.linspace(-camera_vertical_fov / 2, camera_vertical_fov / 2, size) - rays_vertical_angles = np.tile(rays_vertical_angles, (size, 1)).T - - # construct a 2d grid of rays end positions - rays_end_positions_x = camera_max_distance * np.cos(rays_vertical_angles) * np.sin(rays_horizontal_angles) - rays_end_positions_x = rays_end_positions_x.reshape(-1) - rays_end_positions_z = camera_max_distance * np.cos(rays_vertical_angles) * np.cos(rays_horizontal_angles) - rays_end_positions_z = rays_end_positions_z.reshape(-1) - rays_end_positions_y = camera_max_distance * np.sin(rays_vertical_angles) - rays_end_positions_y = rays_end_positions_y.reshape(-1) - rays_end_positions = np.stack((rays_end_positions_x, rays_end_positions_y, rays_end_positions_z), axis=1) - - # transform the rays end positions from camera frame to world frame - cam_to_world_transform = cam_pose.to_transform(camera_frame).get_homogeneous_matrix() - # add the homogeneous coordinate, by adding a column of ones to the position vectors, becoming 4xN matrix - homogenous_end_positions = np.concatenate((rays_end_positions, np.ones((size * size, 1))), axis=1).T - rays_end_positions = cam_to_world_transform @ homogenous_end_positions - rays_end_positions = rays_end_positions[:3, :].T.tolist() - - # apply the ray test - object_ids, distances = self.ray_test_batch(rays_start_positions, rays_end_positions, return_distance=True) - segmentation_mask = np.array(object_ids).squeeze(axis=1).reshape(size, size) - depth_image = np.array(distances).reshape(size, size) + camera_min_distance - normalized_depth_image = (depth_image - camera_min_distance) * 255 / (camera_max_distance - camera_min_distance) - color_depth_image = np.repeat(normalized_depth_image[:, :, np.newaxis], 3, axis=2).astype(np.uint8) - if plot: - self.plot_segmentation_mask(segmentation_mask) - self.plot_depth_image(depth_image) - return [color_depth_image, depth_image, segmentation_mask] - - def plot_segmentation_mask(self, segmentation_mask): - # Create a custom color map - unique_ids = np.unique(segmentation_mask) - unique_ids = unique_ids[unique_ids != -1] # Exclude -1 values - - # Create a color map that assigns a unique color to each ID - colors = plt.cm.get_cmap('tab20', len(unique_ids)) # Use tab20 colormap for distinct colors - color_dict = {uid: colors(i) for i, uid in enumerate(unique_ids)} - - # Map each ID to its corresponding color - segmentation_colored = np.zeros((256, 256, 3)) - - for uid in unique_ids: - segmentation_colored[segmentation_mask == uid] = color_dict[uid][:3] # Ignore the alpha channel - - # Create a colormap for the color bar - cmap = mcolors.ListedColormap([color_dict[uid][:3] for uid in unique_ids]) - norm = mcolors.BoundaryNorm(boundaries=np.arange(len(unique_ids) + 1) - 0.5, ncolors=len(unique_ids)) - - # Plot the colored segmentation mask - fig, ax = plt.subplots() - cax = ax.imshow(segmentation_colored) - ax.axis('off') # Hide axes - ax.set_title('Segmentation Mask with Different Colors for Each Object') - - # Create color bar - cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), ax=ax, ticks=np.arange(len(unique_ids))) - cbar.ax.set_yticklabels([self.get_object_by_id(uid).name for uid in unique_ids]) # Label the color bar with object IDs - cbar.set_label('Object Name') - - plt.show() - - @staticmethod - def plot_depth_image(depth_image): - # Plot the depth image - fig, ax = plt.subplots() - cax = ax.imshow(depth_image, cmap='viridis', vmin=0, vmax=np.max(depth_image)) - ax.axis('off') # Hide axes - ax.set_title('Depth Image') - - # Create color bar - cbar = fig.colorbar(cax, ax=ax) - cbar.set_label('Depth Value') - - plt.show() - def remove_multiverse_resources(self): """ Remove the multiverse resources from the pycram world resources. @@ -276,6 +164,17 @@ def _spawn_floor(self): self.floor = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) + def get_images_for_target(self, target_pose: Pose, + cam_pose: Pose, + size: int = 256, + camera_min_distance: float = 0.1, + camera_max_distance: int = 3, + plot: bool = False) -> List[np.ndarray]: + camera_description = RobotDescription.current_robot_description.get_default_camera() + camera_frame = RobotDescription.current_robot_description.get_camera_frame() + return self.ray_test_utils.get_images_for_target(cam_pose, camera_description, camera_frame, + size, camera_min_distance, camera_max_distance, plot) + @staticmethod def get_joint_position_name(joint: Joint) -> MultiverseJointPosition: return MultiverseJointPosition.from_pycram_joint_type(joint.type) @@ -631,7 +530,7 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], num_threads: int = 1, return_distance: bool = False) -> Union[List[List[int]], - Optional[Tuple[List[List[int]], List[float]]]]: + Optional[Tuple[List[List[int]], List[float]]]]: """ Note: Currently, num_threads is not used in Multiverse. """ diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 6d1c43fb8..4c1b6636e 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -48,7 +48,7 @@ def test_exception(self): @with_tree def failing_plan(): - raise pycram.plan_failures.PlanFailure("PlanFailure for UnitTesting") + raise pycram.failures.PlanFailure("PlanFailure for UnitTesting") pycram.tasktree.reset_tree() From 4ee4910f016f5731867e350e9953dda01e28a670 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 17:43:55 +0200 Subject: [PATCH 130/189] [MultiverseAction] doc for multiverse.py. --- src/pycram/worlds/multiverse.py | 58 ++++++++++++++++++++++++--------- 1 file changed, 43 insertions(+), 15 deletions(-) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6e84c57ae..acbcfcb31 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -177,6 +177,11 @@ def get_images_for_target(self, target_pose: Pose, @staticmethod def get_joint_position_name(joint: Joint) -> MultiverseJointPosition: + """ + Get the attribute name of the joint position in the Multiverse from the pycram joint type. + + :param joint: The joint. + """ return MultiverseJointPosition.from_pycram_joint_type(joint.type) def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: @@ -272,6 +277,10 @@ def _reset_joint_position_using_controller(self, joint: Joint, joint_position: f @validate_multiple_joint_positions def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints in the simulator. Also check if the joint is controlled by an actuator + and use the controller to set the joint position if the joint is controlled. + """ if self.use_controller: controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) @@ -286,15 +295,30 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b return True def get_controlled_joints(self, joints: Optional[List[Joint]] = None) -> List[Joint]: + """ + Get the joints that are controlled by an actuator from the list of joints. + + :param joints: The list of joints to check. + """ joints = self.robot.joints if joints is None else joints return [joint for joint in joints if self.joint_has_actuator(joint)] def _set_multiple_joint_positions_without_controller(self, joint_positions: Dict[Joint, float]) -> None: + """ + Set the positions of multiple joints in the simulator without using the controller. + + :param joint_positions: The dictionary of joints and positions. + """ joints_data = {joint.name: {self.get_joint_position_name(joint): [position]} for joint, position in joint_positions.items()} self.writer.send_multiple_body_data_to_server(joints_data) def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[Joint, float]) -> bool: + """ + Set the positions of multiple joints in the simulator using the controller. + + :param joint_positions: The dictionary of joints and positions. + """ controlled_joints_data = {self.get_actuator_for_joint(joint): {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} @@ -316,6 +340,11 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Optional[Dict[str @staticmethod def get_joint_cmd_name(joint_type: JointType) -> MultiverseJointCMD: + """ + Get the attribute name of the joint command in the Multiverse from the pycram joint type. + + :param joint_type: The pycram joint type. + """ return MultiverseJointCMD.from_pycram_joint_type(joint_type) def get_link_pose(self, link: Link) -> Optional[Pose]: @@ -345,19 +374,6 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: return True - def is_object_a_child_in_a_fixed_joint_constraint(self, obj: Object) -> bool: - """ - Check if the object is a child in a fixed joint constraint. This means that the object is not free to move. - It should be moved according to the parent object. - :param obj: The object to check. - :return: True if the object is a child in a fixed joint constraint, False otherwise. - """ - constraints = list(self.constraints.values()) - for c in constraints: - if c.child_link.object == obj and c.type == JointType.FIXED: - return True - return False - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: """ Reset the poses of multiple objects in the simulator. @@ -409,6 +425,11 @@ def wxyz_to_xyzw(wxyz: List[float]) -> List[float]: return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: + """ + Get the poses of multiple bodies in the simulator. + + :param body_names: The list of body names. + """ return self.reader.get_multiple_body_poses(body_names) def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: @@ -424,10 +445,18 @@ def get_object_orientation(self, obj: Object) -> List[float]: return self.reader.get_body_orientation(obj.name) def multiverse_reset_world(self): + """ + Reset the world using the Multiverse API. + """ self.writer.reset_world() @staticmethod def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: + """ + Convert a quaternion from XYZW to WXYZ format. + + :param xyzw: The quaternion in XYZW format. + """ return [xyzw[3], *xyzw[:3]] def disconnect_from_physics_server(self) -> None: @@ -529,8 +558,7 @@ def ray_test(self, from_position: List[float], to_position: List[float]) -> Opti def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[List[float]], num_threads: int = 1, - return_distance: bool = False) -> Union[List[List[int]], - Optional[Tuple[List[List[int]], List[float]]]]: + return_distance: bool = False) -> Union[List, Tuple[List, List[float]]]: """ Note: Currently, num_threads is not used in Multiverse. """ From 3d24a7109aafbd4ec1b7d40d8659e220f102ef55 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 18:42:10 +0200 Subject: [PATCH 131/189] [MultiverseAction] moved client manager base port class variable value to multiverse_conf.py --- src/pycram/config/multiverse_conf.py | 1 + .../client_manager.py | 33 +++++++++++-------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index b357fd996..25a2447e0 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -3,6 +3,7 @@ HOST: str = "tcp://127.0.0.1" SERVER_HOST: str = HOST SERVER_PORT: str = "7000" +BASE_CLIENT_PORT: int = 9000 simulation_time_step: float = 1e-2 simulation_frequency: int = int(1 / simulation_time_step) diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 9eb0b8f75..5b2363a64 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -3,9 +3,11 @@ from ...worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ MultiverseReader, MultiverseController +from ...config import multiverse_conf as conf + class MultiverseClientManager: - BASE_PORT: int = 9000 + BASE_PORT: int = conf.BASE_CLIENT_PORT """ The base port of the Multiverse client. """ @@ -18,23 +20,26 @@ class MultiverseClientManager: def __init__(self, simulation_wait_time_factor: Optional[float] = 1.0): """ Initialize the Multiverse client manager. - param simulation_wait_time_factor: The simulation wait time factor. + + :param simulation_wait_time_factor: The simulation wait time factor. """ self.simulation_wait_time_factor = simulation_wait_time_factor def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': """ Create a Multiverse reader client. - param is_prospection_world: Whether the reader is connected to the prospection world. + + :param is_prospection_world: Whether the reader is connected to the prospection world. """ return self.create_client(MultiverseReader, "reader", is_prospection_world) def create_writer(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseWriter: """ Create a Multiverse writer client. - param simulation: The name of the simulation that the writer is connected to + + :param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). - param is_prospection_world: Whether the writer is connected to the prospection world. + :param is_prospection_world: Whether the writer is connected to the prospection world. """ return self.create_client(MultiverseWriter, "writer", is_prospection_world, simulation=simulation) @@ -42,17 +47,18 @@ def create_writer(self, simulation: str, is_prospection_world: Optional[bool] = def create_controller(self, is_prospection_world: Optional[bool] = False) -> MultiverseController: """ Create a Multiverse controller client. - param simulation: The name of the simulation that the controller is connected to. - param is_prospection_world: Whether the controller is connected to the prospection world. + + :param is_prospection_world: Whether the controller is connected to the prospection world. """ return self.create_client(MultiverseController, "controller", is_prospection_world) def create_api_requester(self, simulation: str, is_prospection_world: Optional[bool] = False) -> MultiverseAPI: """ Create a Multiverse API client. - param simulation: The name of the simulation that the API is connected to + + :param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). - param is_prospection_world: Whether the API is connected to the prospection world. + :param is_prospection_world: Whether the API is connected to the prospection world. """ return self.create_client(MultiverseAPI, "api_requester", is_prospection_world, simulation=simulation) @@ -64,10 +70,11 @@ def create_client(self, MultiverseReader, MultiverseWriter, MultiverseController]: """ Create a Multiverse client. - param client_type: The type of the client to create. - param name: The name of the client. - param is_prospection_world: Whether the client is connected to the prospection world. - param kwargs: Any other keyword arguments that should be passed to the client constructor. + + :param client_type: The type of the client to create. + :param name: The name of the client. + :param is_prospection_world: Whether the client is connected to the prospection world. + :param kwargs: Any other keyword arguments that should be passed to the client constructor. """ MultiverseClientManager.last_used_port += 1 name = (name or client_type.__name__) + f"_{self.last_used_port}" From 04722836562ae840e47d8865db6d39a731f7ee76 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 19:07:51 +0200 Subject: [PATCH 132/189] [MultiverseAction] doc of socket.py --- .../worlds/multiverse_communication/socket.py | 151 ++++++++---------- 1 file changed, 68 insertions(+), 83 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 67d83a69a..69026f0dd 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -4,6 +4,7 @@ import dataclasses +import rospy from multiverse_client_pybind import MultiverseClientPybind # noqa from typing_extensions import Optional, List, Dict, Callable, TypeVar from ...config import multiverse_conf as conf @@ -33,6 +34,7 @@ def __init__( ) -> None: """ Initialize the MultiverseSocket, connect to the Multiverse Server and start the communication. + :param port: The port of the client. :param host: The host of the client. :param meta_data: The metadata for the Multiverse Client as MultiverseMetaData. @@ -52,82 +54,61 @@ def __init__( "send": {}, "receive": {}, } - self._start_time = 0.0 - - def loginfo(self, message: str) -> None: - """Log information. - - Args: - message (str): The message to log. - - Returns: - None - """ - print(message) + self._api_callbacks: Optional[Dict] = None - def logwarn(self, message: str) -> None: - """Warn the user. - - Args: - message (str): The message to warn about. - - Returns: - None - """ - print(message) + self._start_time = 0.0 def run(self) -> None: - """Run the client. - - Returns: - None - """ - message = f"[Client {self.port}] Start {self.__class__.__name__}{self.port}" - self.loginfo(message) + """Run the client.""" + self.log_info("Start") self._run() def _run(self) -> None: """Run the client, should call the _connect_and_start() method. It's left to the user to implement this method in threaded or non-threaded fashion. - - Returns: - None """ self._connect_and_start() def stop(self) -> None: - """Stop the client. - - Returns: - None - """ + """Stop the client.""" self._disconnect() @property def request_meta_data(self) -> Dict: - """Get the request meta data.""" + """The request_meta_data which is sent to the server. + """ return self._request_meta_data @request_meta_data.setter def request_meta_data(self, request_meta_data: Dict) -> None: - """Set the request_meta_data, make sure to clear the `send` and `receive` field before setting the request""" + """Set the request_meta_data, make sure to clear the `send` and `receive` field before setting the request + """ self._request_meta_data = request_meta_data self._multiverse_socket.set_request_meta_data(self._request_meta_data) @property def response_meta_data(self) -> Dict: - """Get the response_meta_data.""" + """Get the response_meta_data. + + :return: The response_meta_data as a dictionary. + """ response_meta_data = self._multiverse_socket.get_response_meta_data() assert isinstance(response_meta_data, dict) if response_meta_data == {}: message = f"[Client {self.port}] Receive empty response meta data." - self.logwarn(message) + self.log_warn(message) return response_meta_data def send_and_receive_meta_data(self): + """ + Send and receive the meta data, this should be called before sending and receiving data. + """ self._communicate(True) def send_and_receive_data(self): + """ + Send and receive the data, this should be called after sending and receiving the meta data. + """ self._communicate(False) @property @@ -139,7 +120,10 @@ def send_data(self) -> List[float]: def send_data(self, send_data: List[float]) -> None: """Set the send_data, the first element should be the current simulation time, the rest should be the data to send with the following order: - double -> uint8_t -> uint16_t""" + double -> uint8_t -> uint16_t + + :param send_data: The data to send. + """ assert isinstance(send_data, list) self._send_data = send_data self._multiverse_socket.set_send_data(self._send_data) @@ -148,71 +132,65 @@ def send_data(self, send_data: List[float]) -> None: def receive_data(self) -> List[float]: """Get the receive_data, the first element should be the current simulation time, the rest should be the received data with the following order: - double -> uint8_t -> uint16_t""" + double -> uint8_t -> uint16_t + + :return: The received data. + """ receive_data = self._multiverse_socket.get_receive_data() assert isinstance(receive_data, list) return receive_data @property def api_callbacks(self) -> Dict[str, Callable[[List[str]], List[str]]]: - """Get the api_callbacks.""" + """Get the api_callbacks. + + :return: The api_callbacks as a dictionary of function names and their respective callbacks. + """ return self._api_callbacks @api_callbacks.setter def api_callbacks(self, api_callbacks: Dict[str, Callable[[List[str]], List[str]]]) -> None: - """Set the api_callbacks.""" + """Set the api_callbacks. + + :param api_callbacks: The api_callbacks as a dictionary of function names and their respective callbacks. + """ self._multiverse_socket.set_api_callbacks(api_callbacks) self._api_callbacks = api_callbacks def _bind_request_meta_data(self, request_meta_data: T) -> T: """Bind the request_meta_data before sending it to the server. - Args: - request_meta_data (T): The request_meta_data to bind. - - Returns: - T: The binded request_meta_data. + :param request_meta_data: The request_meta_data to bind. + :return: The binded request_meta_data. """ pass def _bind_response_meta_data(self, response_meta_data: T) -> T: """Bind the response_meta_data after receiving it from the server. - Args: - response_meta_data (T): The response_meta_data to bind. - - Returns: - T: The binded response_meta_data. + :param response_meta_data: The response_meta_data to bind. + :return: The binded response_meta_data. """ pass def _bind_send_data(self, send_data: T) -> T: """Bind the send_data before sending it to the server. - Args: - send_data (T): The send_data to bind. - - Returns: - T: The binded send_data. + :param send_data: The send_data to bind. + :return: The binded send_data. """ pass def _bind_receive_data(self, receive_data: T) -> T: """Bind the receive_data after receiving it from the server. - Args: - receive_data (T): The receive_data to bind. - - Returns: - T: The binded receive_data. + :param receive_data: The receive_data to bind. + :return: The binded receive_data. """ pass def _connect_and_start(self) -> None: """Connect to the server and start the client. - - Returns: - None """ self._multiverse_socket.connect(self.host, self.port) self._multiverse_socket.start() @@ -220,38 +198,46 @@ def _connect_and_start(self) -> None: def _disconnect(self) -> None: """Disconnect from the server. - - Returns: - None """ self._multiverse_socket.disconnect() def _communicate(self, resend_request_meta_data: bool = False) -> bool: """Communicate with the server. - Args: - resend_request_meta_data (bool): Resend the request meta data. - - Returns: - bool: True if the communication was successful, False otherwise. + :param resend_request_meta_data: Resend the request meta data. + :return: True if the communication was successful, False otherwise. """ return self._multiverse_socket.communicate(resend_request_meta_data) def _restart(self) -> None: """Restart the client. - - Returns: - None """ self._disconnect() self._connect_and_start() + def log_info(self, message: str) -> None: + """Log information. + + :param message: The message to log. + """ + rospy.loginfo(self._message_template(message)) + + def log_warn(self, message: str) -> None: + """Warn the user. + + :param message: The message to warn about. + """ + rospy.logwarn(self._message_template(message)) + + def _message_template(self, message: str) -> str: + return f"[{self.__class__.__name__}:{self.port}]: {message} : sim time {self.sim_time}, world time {self.world_time}" + + @property def world_time(self) -> float: """Get the world time from the server. - Returns: - float: The world time. + :return: The world time. """ return self._multiverse_socket.get_world_time() @@ -259,7 +245,6 @@ def world_time(self) -> float: def sim_time(self) -> float: """Get the current simulation time. - Returns: - float: The current simulation time. + :return: The current simulation time. """ return self._multiverse_socket.get_time_now() - self._start_time From 9d05b659412eb176a3f5fcbd3ea6dd832752bb13 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 19:31:22 +0200 Subject: [PATCH 133/189] [MultiverseAction] Made remove_object_from_simulator return a boolean value indicating whether the simulator removed the object or not and if true the world would remove the object from its list of objects else it will leave it be. --- src/pycram/datastructures/world.py | 11 +++--- src/pycram/worlds/bullet_world.py | 7 ++-- src/pycram/worlds/multiverse.py | 10 ++++-- .../worlds/multiverse_communication/socket.py | 34 ++++++------------- .../multiverse_datastructures/dataclasses.py | 12 +++++++ 5 files changed, 41 insertions(+), 33 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index e4493ed6b..ef4e1e359 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -360,20 +360,22 @@ def get_object_by_id(self, obj_id: int) -> Object: return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] @abstractmethod - def remove_object_by_id(self, obj_id: int) -> None: + def remove_object_by_id(self, obj_id: int) -> bool: """ Removes the object with the given id from the world. :param obj_id: The unique id of the object to be removed. + :return: Whether the object was removed successfully. """ pass @abstractmethod - def remove_object_from_simulator(self, obj: Object) -> None: + def remove_object_from_simulator(self, obj: Object) -> bool: """ Removes an object from the physics simulator. :param obj: The object to be removed. + :return: Whether the object was removed successfully. """ pass @@ -389,9 +391,10 @@ def remove_object(self, obj: Object) -> None: self.object_lock.acquire() obj.detach_all() - if obj.name != "floor": + + if self.remove_object_from_simulator(obj): self.objects.remove(obj) - self.remove_object_from_simulator(obj) + if World.robot == obj: World.robot = None diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 7059b2850..ded66cbb1 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -83,11 +83,12 @@ def _load_object_and_get_id(self, path: str, pose: Pose) -> int: basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) - def remove_object_from_simulator(self, obj: Object) -> None: - p.removeBody(obj.id, self.id) + def remove_object_from_simulator(self, obj: Object) -> bool: + return self.remove_object_by_id(obj.id) - def remove_object_by_id(self, obj_id: int) -> None: + def remove_object_by_id(self, obj_id: int) -> True: p.removeBody(obj_id, self.id) + return True def add_constraint(self, constraint: Constraint) -> int: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index acbcfcb31..387a08d0e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,6 +3,7 @@ from time import sleep import numpy as np +import rospy from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional, Union, Tuple, Callable @@ -466,13 +467,16 @@ def join_threads(self) -> None: self.reader.stop_thread = True self.reader.join() - def remove_object_by_id(self, obj_id: int) -> None: + def remove_object_by_id(self, obj_id: int) -> bool: obj = self.get_object_by_id(obj_id) - self.remove_object_from_simulator(obj) + return self.remove_object_from_simulator(obj) - def remove_object_from_simulator(self, obj: Object) -> None: + def remove_object_from_simulator(self, obj: Object) -> bool: if obj.obj_type != ObjectType.ENVIRONMENT: self.writer.remove_body(obj.name) + return True + rospy.logwarn("Cannot remove environment objects") + return False def add_constraint(self, constraint: Constraint) -> int: diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 69026f0dd..f63624e2d 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -2,28 +2,16 @@ """Multiverse Client base class.""" -import dataclasses - import rospy from multiverse_client_pybind import MultiverseClientPybind # noqa from typing_extensions import Optional, List, Dict, Callable, TypeVar + +from ..multiverse_datastructures.dataclasses import MultiverseMetaData from ...config import multiverse_conf as conf T = TypeVar("T") -@dataclasses.dataclass -class MultiverseMetaData: - """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" - world_name: str = "world" - simulation_name: str = "cram" - length_unit: str = "m" - angle_unit: str = "rad" - mass_unit: str = "kg" - time_unit: str = "s" - handedness: str = "rhs" - - class MultiverseSocket: def __init__( @@ -101,13 +89,13 @@ def response_meta_data(self) -> Dict: def send_and_receive_meta_data(self): """ - Send and receive the meta data, this should be called before sending and receiving data. + Send and receive the metadata, this should be called before sending and receiving data. """ self._communicate(True) def send_and_receive_data(self): """ - Send and receive the data, this should be called after sending and receiving the meta data. + Send and receive the data, this should be called after sending and receiving the metadata. """ self._communicate(False) @@ -161,7 +149,7 @@ def _bind_request_meta_data(self, request_meta_data: T) -> T: """Bind the request_meta_data before sending it to the server. :param request_meta_data: The request_meta_data to bind. - :return: The binded request_meta_data. + :return: The bound request_meta_data. """ pass @@ -169,7 +157,7 @@ def _bind_response_meta_data(self, response_meta_data: T) -> T: """Bind the response_meta_data after receiving it from the server. :param response_meta_data: The response_meta_data to bind. - :return: The binded response_meta_data. + :return: The bound response_meta_data. """ pass @@ -177,7 +165,7 @@ def _bind_send_data(self, send_data: T) -> T: """Bind the send_data before sending it to the server. :param send_data: The send_data to bind. - :return: The binded send_data. + :return: The bound send_data. """ pass @@ -185,7 +173,7 @@ def _bind_receive_data(self, receive_data: T) -> T: """Bind the receive_data after receiving it from the server. :param receive_data: The receive_data to bind. - :return: The binded receive_data. + :return: The bound receive_data. """ pass @@ -204,7 +192,7 @@ def _disconnect(self) -> None: def _communicate(self, resend_request_meta_data: bool = False) -> bool: """Communicate with the server. - :param resend_request_meta_data: Resend the request meta data. + :param resend_request_meta_data: Resend the request metadata. :return: True if the communication was successful, False otherwise. """ return self._multiverse_socket.communicate(resend_request_meta_data) @@ -230,8 +218,8 @@ def log_warn(self, message: str) -> None: rospy.logwarn(self._message_template(message)) def _message_template(self, message: str) -> str: - return f"[{self.__class__.__name__}:{self.port}]: {message} : sim time {self.sim_time}, world time {self.world_time}" - + return (f"[{self.__class__.__name__}:{self.port}]: {message} : sim time {self.sim_time}," + f" world time {self.world_time}") @property def world_time(self) -> float: diff --git a/src/pycram/worlds/multiverse_datastructures/dataclasses.py b/src/pycram/worlds/multiverse_datastructures/dataclasses.py index 914ea0628..85f94b2c1 100644 --- a/src/pycram/worlds/multiverse_datastructures/dataclasses.py +++ b/src/pycram/worlds/multiverse_datastructures/dataclasses.py @@ -3,6 +3,18 @@ from typing_extensions import List +@dataclass +class MultiverseMetaData: + """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" + world_name: str = "world" + simulation_name: str = "cram" + length_unit: str = "m" + angle_unit: str = "rad" + mass_unit: str = "kg" + time_unit: str = "s" + handedness: str = "rhs" + + @dataclass class RayResult: """ From 023396bc22adfa3a20756f4f150af20be13c9c35 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 19:48:22 +0200 Subject: [PATCH 134/189] [MultiverseAction] clients doc. --- src/pycram/config/multiverse_conf.py | 10 + .../multiverse_communication/clients.py | 378 ++++++++++-------- 2 files changed, 227 insertions(+), 161 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 25a2447e0..011ba58db 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -1,10 +1,20 @@ +import datetime + from . import world_conf as conf +# Multiverse Socket Configuration HOST: str = "tcp://127.0.0.1" SERVER_HOST: str = HOST SERVER_PORT: str = "7000" BASE_CLIENT_PORT: int = 9000 +# Multiverse Client Configuration +READER_MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) +""" +The maximum wait time for the data in seconds. +""" + +# Multiverse Simulation Configuration simulation_time_step: float = 1e-2 simulation_frequency: int = int(1 / simulation_time_step) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 885bbd774..e632d48d9 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -13,18 +13,20 @@ from ...datastructures.world import World from ...world_concepts.constraints import Constraint from ...world_concepts.world_object import Object, Link +from ...config import multiverse_conf as conf class MultiverseClient(MultiverseSocket): - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, - simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): """ Initialize the Multiverse client, which connects to the Multiverse server. - param name: The name of the client. - param port: The port of the client. - param is_prospection_world: Whether the client is connected to the prospection world. - param simulation_wait_time_factor: The simulation wait time factor (default is 1.0), which can be used to + + :param name: The name of the client. + :param port: The port of the client. + :param is_prospection_world: Whether the client is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor (default is 1.0), which can be used to increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() @@ -38,19 +40,20 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) + MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = conf.READER_MAX_WAIT_TIME_FOR_DATA """ The maximum wait time for the data in seconds. """ - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, - simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): """ Initialize the Multiverse reader, which reads the data from the Multiverse server in a separate thread. This class provides methods to get data (e.g., position, orientation) from the Multiverse server. - param port: The port of the Multiverse reader client. - param is_prospection_world: Whether the reader is connected to the prospection world. - param simulation_wait_time_factor: The simulation wait time factor. + + :param port: The port of the Multiverse reader client. + :param is_prospection_world: Whether the reader is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor. """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) @@ -62,21 +65,23 @@ def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = self.thread.start() - def get_body_pose(self, name: str, wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: + def get_body_pose(self, name: str, wait: bool = False) -> Optional[Dict[str, List[float]]]: """ Get the body pose from the multiverse server. - param name: The name of the body. - param wait: Whether to wait for the data. - return: The position and orientation of the body. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The position and orientation of the body. """ return self.get_body_data(name, [BodyProperty.POSITION, BodyProperty.ORIENTATION], wait=wait) - def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = False) -> Optional[Dict[str, Pose]]: + def get_multiple_body_poses(self, body_names: List[str], wait: bool = False) -> Optional[Dict[str, Pose]]: """ Get the body poses from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param wait: Whether to wait for the data. - return: The positions and orientations of the bodies as a dictionary. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The positions and orientations of the bodies as a dictionary. """ data = self.get_multiple_body_data(body_names, {name: [BodyProperty.POSITION, BodyProperty.ORIENTATION] @@ -88,66 +93,72 @@ def get_multiple_body_poses(self, body_names: List[str], wait: Optional[bool] = self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) for name in body_names} - def get_body_position(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: + def get_body_position(self, name: str, wait: bool = False) -> Optional[List[float]]: """ Get the body position from the multiverse server. - param name: The name of the body. - param wait: Whether to wait for the data. - return: The position of the body. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The position of the body. """ return self.get_body_property(name, BodyProperty.POSITION, wait=wait) def get_multiple_body_positions(self, body_names: List[str], - wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: + wait: bool = False) -> Optional[Dict[str, List[float]]]: """ Get the body positions from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param wait: Whether to wait for the data. - return: The positions of the bodies as a dictionary. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The positions of the bodies as a dictionary. """ return self.get_multiple_body_properties(body_names, [BodyProperty.POSITION], wait=wait) - def get_body_orientation(self, name: str, wait: Optional[bool] = False) -> Optional[List[float]]: + def get_body_orientation(self, name: str, wait: bool = False) -> Optional[List[float]]: """ Get the body orientation from the multiverse server. - param name: The name of the body. - param wait: Whether to wait for the data. - return: The orientation of the body. + + :param name: The name of the body. + :param wait: Whether to wait for the data. + :return: The orientation of the body. """ return self.get_body_property(name, BodyProperty.ORIENTATION, wait=wait) def get_multiple_body_orientations(self, body_names: List[str], - wait: Optional[bool] = False) -> Optional[Dict[str, List[float]]]: + wait: bool = False) -> Optional[Dict[str, List[float]]]: """ Get the body orientations from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param wait: Whether to wait for the data. - return: The orientations of the bodies as a dictionary. + + :param body_names: The names of the bodies. + :param wait: Whether to wait for the data. + :return: The orientations of the bodies as a dictionary. """ data = self.get_multiple_body_properties(body_names, [BodyProperty.ORIENTATION], wait=wait) if data is not None: return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} - def get_body_property(self, name: str, property_: Property, wait: Optional[bool] = False) -> Optional[List[float]]: + def get_body_property(self, name: str, property_: Property, wait: bool = False) -> Optional[List[float]]: """ Get the body property from the multiverse server. - param name: The name of the body. - param property_: The property of the body as a Property. - param wait: Whether to wait for the data. - return: The property of the body. + + :param name: The name of the body. + :param property_: The property of the body as a Property. + :param wait: Whether to wait for the data. + :return: The property of the body. """ data = self.get_body_data(name, [property_], wait=wait) if data is not None: return data[property_.value] def get_multiple_body_properties(self, body_names: List[str], properties: List[Property], - wait: Optional[bool] = False) -> Optional[Dict[str, Dict[str, List[float]]]]: + wait: bool = False) -> Optional[Dict[str, Dict[str, List[float]]]]: """ Get the body properties from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param properties: The properties of the bodies. - param wait: Whether to wait for the data. - return: The properties of the bodies as a dictionary. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param wait: Whether to wait for the data. + :return: The properties of the bodies as a dictionary. """ return self.get_multiple_body_data(body_names, {name: properties for name in body_names}, wait=wait) @@ -155,20 +166,22 @@ def get_multiple_body_properties(self, body_names: List[str], properties: List[P def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: """ Convert the quaternion from wxyz to xyzw. - param quaternion: The quaternion as a list of floats. - return: The quaternion as a list of floats. + + :param quaternion: The quaternion as a list of floats. + :return: The quaternion as a list of floats. """ return quaternion[1:] + [quaternion[0]] def get_body_data(self, name: str, properties: Optional[List[Property]] = None, - wait: Optional[bool] = False) -> Optional[Dict]: + wait: bool = False) -> Optional[Dict]: """ Get the body data from the multiverse server. - param name: The name of the body. - param properties: The properties of the body. - param wait: Whether to wait for the data. - return: The body data as a dictionary. + + :param name: The name of the body. + :param properties: The properties of the body. + :param wait: Whether to wait for the data. + :return: The body data as a dictionary. """ if wait: return self.wait_for_body_data(name, properties) @@ -179,13 +192,14 @@ def get_body_data(self, name: str, def get_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[Property]]] = None, - wait: Optional[bool] = False) -> Optional[Dict]: + wait: bool = False) -> Optional[Dict]: """ Get the body data from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param properties: The properties of the bodies. - param wait: Whether to wait for the data. - return: The body data as a dictionary. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param wait: Whether to wait for the data. + :return: The body data as a dictionary. """ if wait: @@ -198,9 +212,10 @@ def get_multiple_body_data(self, body_names: List[str], def wait_for_body_data(self, name: str, properties: Optional[List[Property]] = None) -> Dict: """ Wait for the body data from the multiverse server. - param name: The name of the body. - param properties: The properties of the body. - return: The body data as a dictionary. + + :param name: The name of the body. + :param properties: The properties of the body. + :return: The body data as a dictionary. """ return self._wait_for_body_data_template(name, self.check_for_body_data, properties)[name] @@ -208,9 +223,10 @@ def wait_for_multiple_body_data(self, body_names: List[str], properties: Optional[Dict[str, List[Property]]] = None) -> Dict: """ Wait for the body data from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param properties: The properties of the bodies. - return: The body data as a dictionary. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :return: The body data as a dictionary. """ return self._wait_for_body_data_template(body_names, self.check_multiple_body_data, properties) @@ -219,11 +235,11 @@ def _wait_for_body_data_template(self, body_names: Union[str, List[str]], properties: Optional[Union[Dict, List]] = None) -> Dict: """ Wait for the body data from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param properties: The properties of the bodies. - param check_func: The function to check if the data is received. - param return_func: The function to return the data. - return: The body data as a dictionary. + + :param body_names: The names of the bodies. + :param properties: The properties of the bodies. + :param check_func: The function to check if the data is received. + :return: The body data as a dictionary. """ start = time() data_received_flag = False @@ -242,10 +258,11 @@ def check_multiple_body_data(self, body_names: List[str], data: Dict, properties: Optional[Dict[str, List[Property]]] = None) -> bool: """ Check if the body data is received from the multiverse server for multiple bodies. - param body_names: The names of the bodies. - param data: The data received from the multiverse server. - param properties: The properties of the bodies. - return: Whether the body data is received. + + :param body_names: The names of the bodies. + :param data: The data received from the multiverse server. + :param properties: The properties of the bodies. + :return: Whether the body data is received. """ if properties is None: return all([self.check_for_body_data(name, data) for name in body_names]) @@ -256,10 +273,11 @@ def check_multiple_body_data(self, body_names: List[str], data: Dict, def check_for_body_data(name: str, data: Dict, properties: Optional[List[Property]] = None) -> bool: """ Check if the body data is received from the multiverse server. - param name: The name of the body. - param data: The data received from the multiverse server. - param properties: The properties of the body. - return: Whether the body data is received. + + :param name: The name of the body. + :param data: The data received from the multiverse server. + :param properties: The properties of the body. + :return: Whether the body data is received. """ if properties is None: return name in data @@ -295,16 +313,17 @@ def join(self): class MultiverseWriter(MultiverseClient): def __init__(self, name: str, port: int, simulation: Optional[str] = None, - is_prospection_world: Optional[bool] = False, - simulation_wait_time_factor: Optional[float] = 1.0, **kwargs): + is_prospection_world: bool = False, + simulation_wait_time_factor: float = 1.0, **kwargs): """ Initialize the Multiverse writer, which writes the data to the Multiverse server. This class provides methods to send data (e.g., position, orientation) to the Multiverse server. - param port: The port of the Multiverse writer client. - param simulation: The name of the simulation that the writer is connected to + + :param port: The port of the Multiverse writer client. + :param simulation: The name of the simulation that the writer is connected to (usually the name defined in the .muv file). - param is_prospection_world: Whether the writer is connected to the prospection world. - param simulation_wait_time_factor: The wait time factor for the simulation (default is 1.0), which can be used + :param is_prospection_world: Whether the writer is connected to the prospection world. + :param simulation_wait_time_factor: The wait time factor for the simulation (default is 1.0), which can be used to increase or decrease the wait time for the simulation. """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) @@ -314,6 +333,7 @@ def spawn_robot_with_actuators(self, robot_name: str, position: List[float], ori actuator_joint_commands: Optional[Dict[str, List[str]]] = None) -> None: """ Spawn the robot with controlled actuators in the simulation. + :param robot_name: The name of the robot. :param position: The position of the robot. :param orientation: The orientation of the robot. @@ -340,9 +360,10 @@ def _reset_request_meta_data(self): def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]) -> None: """ Set the body pose in the simulation. - param body_name: The name of the body. - param position: The position of the body. - param orientation: The orientation of the body. + + :param body_name: The name of the body. + :param position: The position of the body. + :param orientation: The orientation of the body. """ self.send_body_data_to_server(body_name, {BodyProperty.POSITION: position, @@ -352,39 +373,44 @@ def set_body_pose(self, body_name: str, position: List[float], orientation: List def set_multiple_body_poses(self, body_data: Dict[str, Dict[BodyProperty, List[float]]]) -> None: """ Set the body poses in the simulation for multiple bodies. - param body_data: The data to be sent for multiple bodies. + + :param body_data: The data to be sent for multiple bodies. """ self.send_multiple_body_data_to_server(body_data) def set_body_position(self, body_name: str, position: List[float]) -> None: """ Set the body position in the simulation. - param body_name: The name of the body. - param position: The position of the body. + + :param body_name: The name of the body. + :param position: The position of the body. """ self.set_body_property(body_name, BodyProperty.POSITION, position) def set_body_orientation(self, body_name: str, orientation: List[float]) -> None: """ Set the body orientation in the simulation. - param body_name: The name of the body. - param orientation: The orientation of the body. + + :param body_name: The name of the body. + :param orientation: The orientation of the body. """ self.set_body_property(body_name, BodyProperty.ORIENTATION, orientation) def set_body_property(self, body_name: str, property_: Property, value: List[float]) -> None: """ Set the body property in the simulation. - param body_name: The name of the body. - param property_: The property of the body. - param value: The value of the property. + + :param body_name: The name of the body. + :param property_: The property of the body. + :param value: The value of the property. """ self.send_body_data_to_server(body_name, {property_: value}) def remove_body(self, body_name: str) -> None: """ Remove the body from the simulation. - param body_name: The name of the body. + + :param body_name: The name of the body. """ self.send_data_to_server([self.sim_time], send_meta_data={body_name: []}, @@ -399,9 +425,10 @@ def reset_world(self) -> None: def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, List[float]]) -> Dict: """ Send data to the multiverse server. - param body_name: The name of the body. - param body_data: The data to be sent. - return: The response from the server. + + :param body_name: The name of the body. + :param body_data: The data to be sent. + :return: The response from the server. """ send_meta_data = {body_name: list(map(str, body_data.keys()))} flattened_data = [value for data in body_data.values() for value in data] @@ -410,8 +437,9 @@ def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, Lis def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ Send data to the multiverse server for multiple bodies. - param body_data: The data to be sent for multiple bodies. - return: The response from the server. + + :param body_data: The data to be sent for multiple bodies. + :return: The response from the server. """ send_meta_data = {body_name: list(map(str, data.keys())) for body_name, data in body_data.items()} response_meta_data = self.send_meta_data_and_get_response(send_meta_data) @@ -425,8 +453,9 @@ def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: """ Send metadata to the multiverse server and get the response. - param send_meta_data: The metadata to be sent. - return: The response from the server. + + :param send_meta_data: The metadata to be sent. + :return: The response from the server. """ self._reset_request_meta_data() self.request_meta_data["send"] = send_meta_data @@ -438,10 +467,11 @@ def send_data_to_server(self, data: List, receive_meta_data: Optional[Dict] = None) -> Dict: """ Send data to the multiverse server. - param data: The data to be sent. - param send_meta_data: The metadata to be sent. - param receive_meta_data: The metadata to be received. - return: The response from the server. + + :param data: The data to be sent. + :param send_meta_data: The metadata to be sent. + :param receive_meta_data: The metadata to be received. + :return: The response from the server. """ self._reset_request_meta_data() if send_meta_data: @@ -456,18 +486,20 @@ def send_data_to_server(self, data: List, class MultiverseController(MultiverseWriter): - def __init__(self, name: str, port: int, is_prospection_world: Optional[bool] = False, **kwargs): + def __init__(self, name: str, port: int, is_prospection_world: bool = False, **kwargs): """ Initialize the Multiverse controller, which controls the robot in the simulation. This class provides methods to send controller data to the Multiverse server. - param port: The port of the Multiverse controller client. - param is_prospection_world: Whether the controller is connected to the prospection world. + + :param port: The port of the Multiverse controller client. + :param is_prospection_world: Whether the controller is connected to the prospection world. """ super().__init__(name, port, is_prospection_world=is_prospection_world) def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None: """ Initialize the controller by sending the controller data to the multiverse server. + :param actuator_joint_commands: A dictionary mapping actuator names to joint command names. """ self.send_data_to_server([self.sim_time] + [0.0] * len(actuator_joint_commands), @@ -482,26 +514,28 @@ class MultiverseAPI(MultiverseClient): """ APIs_THAT_NEED_WAIT_TIME: List[API] = [API.ATTACH] - def __init__(self, name: str, port: int, simulation: str, is_prospection_world: Optional[bool] = False, + def __init__(self, name: str, port: int, simulation: str, is_prospection_world: bool = False, simulation_wait_time_factor: float = 1.0): """ Initialize the Multiverse API, which sends API requests to the Multiverse server. This class provides methods like attach and detach objects, get contact points, and other API requests. - param port: The port of the Multiverse API client. - param simulation: The name of the simulation that the API is connected to + + :param port: The port of the Multiverse API client. + :param simulation: The name of the simulation that the API is connected to (usually the name defined in the .muv file). - param is_prospection_world: Whether the API is connected to the prospection world. - param simulation_wait_time_factor: The simulation wait time factor, which can be used to increase or decrease + :param is_prospection_world: Whether the API is connected to the prospection world. + :param simulation_wait_time_factor: The simulation wait time factor, which can be used to increase or decrease the wait time for the simulation. """ super().__init__(name, port, is_prospection_world, simulation_wait_time_factor=simulation_wait_time_factor) self.simulation = simulation - self.wait: Optional[bool] = False # Whether to wait after sending the API request. + self.wait: bool = False # Whether to wait after sending the API request. def attach(self, constraint: Constraint) -> None: """ Request to attach the child link to the parent link. - param constraint: The constraint. + + :param constraint: The constraint. """ self.wait = True parent_link_name, child_link_name = self.get_constraint_link_names(constraint) @@ -511,9 +545,10 @@ def attach(self, constraint: Constraint) -> None: def _attach(self, child_link_name: str, parent_link_name: str, attachment_pose: str) -> None: """ Attach the child link to the parent link. - param child_link_name: The name of the child link. - param parent_link_name: The name of the parent link. - param attachment_pose: The attachment pose. + + :param child_link_name: The name of the child link. + :param parent_link_name: The name of the parent link. + :param attachment_pose: The attachment pose. """ self._request_single_api_callback(API.ATTACH, child_link_name, parent_link_name, attachment_pose) @@ -521,24 +556,27 @@ def _attach(self, child_link_name: str, parent_link_name: str, attachment_pose: def get_constraint_link_names(self, constraint: Constraint) -> Tuple[str, str]: """ Get the link names of the constraint. - param constraint: The constraint. - return: The link names of the constraint. + + :param constraint: The constraint. + :return: The link names of the constraint. """ return self.get_parent_link_name(constraint), self.get_constraint_child_link_name(constraint) def get_parent_link_name(self, constraint: Constraint) -> str: """ Get the parent link name of the constraint. - param constraint: The constraint. - return: The parent link name of the constraint. + + :param constraint: The constraint. + :return: The parent link name of the constraint. """ return self.get_link_name_for_constraint(constraint.parent_link) def get_constraint_child_link_name(self, constraint: Constraint) -> str: """ Get the child link name of the constraint. - param constraint: The constraint. - return: The child link name of the constraint. + + :param constraint: The constraint. + :return: The child link name of the constraint. """ return self.get_link_name_for_constraint(constraint.child_link) @@ -546,15 +584,17 @@ def get_constraint_child_link_name(self, constraint: Constraint) -> str: def get_link_name_for_constraint(link: Link) -> str: """ Get the link name from link object, if the link belongs to a one link object, return the object name. - param link: The link. - return: The link name. + + :param link: The link. + :return: The link name. """ return link.name if not link.is_only_link else link.object.name def detach(self, constraint: Constraint) -> None: """ Request to detach the child link from the parent link. - param constraint: The constraint. + + :param constraint: The constraint. """ parent_link_name, child_link_name = self.get_constraint_link_names(constraint) self._detach(child_link_name, parent_link_name) @@ -562,16 +602,18 @@ def detach(self, constraint: Constraint) -> None: def _detach(self, child_link_name: str, parent_link_name: str) -> None: """ Detach the child link from the parent link. - param child_link_name: The name of the child link. - param parent_link_name: The name of the parent link. + + :param child_link_name: The name of the child link. + :param parent_link_name: The name of the parent link. """ self._request_single_api_callback(API.DETACH, child_link_name, parent_link_name) def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: """ Get the attachment pose as a string. - param constraint: The constraint. - return: The attachment pose as a string. + + :param constraint: The constraint. + :return: The attachment pose as a string. """ pose = constraint.parent_to_child_transform.to_pose() return self._pose_to_string(pose) @@ -580,8 +622,9 @@ def _get_attachment_pose_as_string(self, constraint: Constraint) -> str: def _pose_to_string(pose: Pose) -> str: """ Convert the pose to a string. - param pose: The pose. - return: The pose as a string. + + :param pose: The pose. + :return: The pose as a string. """ return f"{pose.position.x} {pose.position.y} {pose.position.z} {pose.orientation.w} {pose.orientation.x} " \ f"{pose.orientation.y} {pose.orientation.z}" @@ -589,24 +632,27 @@ def _pose_to_string(pose: Pose) -> str: def check_object_exists(self, obj: Object) -> bool: """ Check if the object exists in the simulation. - param obj: The object. - return: Whether the object exists in the simulation. + + :param obj: The object. + :return: Whether the object exists in the simulation. """ return self._object_exists(obj.name) def _object_exists(self, object_name: str) -> bool: """ Check if the object exists in the simulation. - param object_name: The name of the object. - return: Whether the object exists in the simulation. + + :param object_name: The name of the object. + :return: Whether the object exists in the simulation. """ return self._request_single_api_callback(API.EXIST, object_name)[0] == 'yes' def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: """ Request the contact points of an object, this includes the object names and the contact forces and torques. - param obj: The object. - return: The contact points of the object as a list of MultiverseContactPoint. + + :param obj: The object. + :return: The contact points of the object as a list of MultiverseContactPoint. """ api_response_data = self._get_contact_points(obj.name) body_names = api_response_data[API.GET_CONTACT_BODIES] @@ -618,9 +664,10 @@ def get_objects_intersected_with_rays(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> List[RayResult]: """ Get the rays intersections with the objects from the from_positions to the to_positions. - param from_positions: The starting positions of the rays. - param to_positions: The ending positions of the rays. - return: The rays intersections with the objects as a list of RayResult. + + :param from_positions: The starting positions of the rays. + :param to_positions: The ending positions of the rays. + :return: The rays intersections with the objects as a list of RayResult. """ get_rays_response = self._get_rays(from_positions, to_positions) return self._parse_get_rays_response(get_rays_response) @@ -629,9 +676,10 @@ def _get_rays(self, from_positions: List[List[float]], to_positions: List[List[float]]) -> List[str]: """ Get the rays intersections with the objects from the from_positions to the to_positions. - param from_positions: The starting positions of the rays. - param to_positions: The ending positions of the rays. - return: The rays intersections with the objects as a dictionary. + + :param from_positions: The starting positions of the rays. + :param to_positions: The ending positions of the rays. + :return: The rays intersections with the objects as a dictionary. """ from_positions = self.list_of_positions_to_string(from_positions) to_positions = self.list_of_positions_to_string(to_positions) @@ -641,8 +689,9 @@ def _get_rays(self, from_positions: List[List[float]], def _parse_get_rays_response(response: List[str]) -> List[RayResult]: """ Parse the response of the get rays API. - param response: The response of the get rays API as a list of strings. - return: The rays as a list of lists of floats. + + :param response: The response of the get rays API as a list of strings. + :return: The rays as a list of lists of floats. """ get_rays_results = [] for ray_response in response: @@ -658,8 +707,9 @@ def _parse_get_rays_response(response: List[str]) -> List[RayResult]: def list_of_positions_to_string(positions: List[List[float]]) -> str: """ Convert the list of positions to a string. - param positions: The list of positions. - return: The list of positions as a string. + + :param positions: The list of positions. + :return: The list of positions as a string. """ return " ".join([f"{position[0]} {position[1]} {position[2]}" for position in positions]) @@ -667,16 +717,18 @@ def list_of_positions_to_string(positions: List[List[float]]) -> str: def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: """ Parse the contact effort of an object. - param contact_effort: The contact effort of the object as a list of strings. - return: The contact effort of the object as a list of floats. + + :param contact_effort: The contact effort of the object as a list of strings. + :return: The contact effort of the object as a list of floats. """ return list(map(float, contact_effort[0].split())) def _get_contact_points(self, object_name) -> Dict[API, List]: """ Request the contact points of an object. - param object_name: The name of the object. - return: The contact points api response as a dictionary. + + :param object_name: The name of the object. + :return: The contact points api response as a dictionary. """ return self._request_apis_callbacks({API.GET_CONTACT_BODIES: [object_name], API.GET_CONSTRAINT_EFFORT: [object_name] @@ -697,8 +749,9 @@ def unpause_simulation(self) -> None: def _request_single_api_callback(self, api_name: API, *params) -> List[str]: """ Request a single API callback from the server. - param api_data: The API data to request the callback. - return: The API response as a list of strings. + + :param api_data: The API data to request the callback. + :return: The API response as a list of strings. """ response = self._request_apis_callbacks({api_name: list(params)}) return response[api_name] @@ -706,8 +759,9 @@ def _request_single_api_callback(self, api_name: API, *params) -> List[str]: def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[str]]: """ Request the API callbacks from the server. - param api_data_request: The API data to add to the request metadata. - return: The API response as a list of strings. + + :param api_data: The API data to add to the request metadata. + :return: The API response as a list of strings. """ self._reset_api_callback() for api_name, params in api_data.items(): @@ -722,7 +776,8 @@ def _request_apis_callbacks(self, api_data: Dict[API, List]) -> Dict[API, List[s def _get_all_apis_responses(self) -> Dict[API, List[str]]: """ Get all the API responses from the server. - return: The API responses as a list of APIData. + + :return: The API responses as a list of APIData. """ list_of_api_responses = self.response_meta_data["api_callbacks_response"][self.simulation] return {API[api_name.upper()]: response for api_response in list_of_api_responses @@ -731,8 +786,9 @@ def _get_all_apis_responses(self) -> Dict[API, List[str]]: def _add_api_request(self, api_name: str, *params): """ Add an API request to the request metadata. - param api_name: The name of the API. - param params: The parameters of the API. + + :param api_name: The name of the API. + :param params: The parameters of the API. """ self.request_meta_data["api_callbacks"][self.simulation].append({api_name: list(params)}) From d86aa24c4093bcb0165c6cae41168a97f5865637 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 19:51:50 +0200 Subject: [PATCH 135/189] [MultiverseAction] clean clients.py --- src/pycram/worlds/multiverse_communication/clients.py | 11 +---------- test/test_multiverse.py | 6 ++++-- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index e632d48d9..dfa740f18 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -636,16 +636,7 @@ def check_object_exists(self, obj: Object) -> bool: :param obj: The object. :return: Whether the object exists in the simulation. """ - return self._object_exists(obj.name) - - def _object_exists(self, object_name: str) -> bool: - """ - Check if the object exists in the simulation. - - :param object_name: The name of the object. - :return: Whether the object exists in the simulation. - """ - return self._request_single_api_callback(API.EXIST, object_name)[0] == 'yes' + return self._request_single_api_callback(API.EXIST, obj.name)[0] == 'yes' def get_contact_points(self, obj: Object) -> List[MultiverseContactPoint]: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ad85182c0..a9392bd7f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -184,7 +184,7 @@ def test_set_robot_position(self): step = -1 for i in range(3): self.spawn_robot() - new_position = [-3 + step*i, -3 + step*i, 0.001] + new_position = [-3 + step * i, -3 + step * i, 0.001] self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], @@ -297,7 +297,9 @@ 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.1]) - self.multiverse.simulate(0.1) + # 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.2) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From ce06f1ca2fa3e091a4c951e3c0e55b32c6c70726 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 20:34:48 +0200 Subject: [PATCH 136/189] [MultiverseAction] Moved set_mobile_robot_base_pose to world_object.py and cleaned the doc. --- src/pycram/datastructures/pose.py | 28 +++++ src/pycram/datastructures/world.py | 65 +----------- src/pycram/world_concepts/world_object.py | 123 ++++++++++++++++++---- src/pycram/worlds/multiverse.py | 4 +- 4 files changed, 137 insertions(+), 83 deletions(-) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index a8feade26..71530fff7 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -3,6 +3,8 @@ import math import datetime + +from tf.transformations import euler_from_quaternion from typing_extensions import List, Union, Optional, Sized import numpy as np @@ -86,6 +88,32 @@ def from_pose_stamped(pose_stamped: PoseStamped) -> Pose: p.pose = pose_stamped.pose return p + def get_position_diff(self, target_pose: 'Pose') -> Point: + """ + Get the difference between the target and the current positions. + + :param target_pose: The target pose. + :return: The difference between the two positions. + """ + return Point(target_pose.position.x - self.position.x, target_pose.position.y - self.position.y, + target_pose.position.z - self.position.z) + + def get_z_angle_difference(self, target_pose: Pose) -> float: + """ + Get the difference between two z angles. + + :param target_pose: The target pose. + :return: The difference between the two z angles. + """ + return target_pose.z_angle - self.z_angle + + @property + def z_angle(self) -> float: + """ + The z angle of the orientation of this Pose in radians. + """ + return euler_from_quaternion(self.orientation_as_list())[2] + @property def frame(self) -> str: """ diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ef4e1e359..679886726 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -615,49 +615,11 @@ def robot_virtual_joints(self) -> List[Joint]: @property def robot_virtual_joints_names(self) -> List[str]: """ - Returns the virtual joints of the robot. + Return the virtual joints of the robot. """ return self.robot_description.virtual_move_base_joints.names - def set_mobile_robot_pose(self, robot_obj: Object, pose: Pose) -> None: - """ - Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when - the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). - :param robot_obj: The robot object. - :param pose: The target pose. - """ - goal = self.get_move_base_joint_goal(robot_obj, pose) - robot_obj.set_multiple_joint_positions(goal) - - def get_move_base_joint_goal(self, robot_obj: Object, pose: Pose) -> Dict[str, float]: - """ - Get the goal for the move base joints of a mobile robot to reach a target pose. - :param robot_obj: The robot object. - :param pose: The target pose. - return: The goal for the move base joints. - """ - position_diff = self.get_position_diff(robot_obj.original_pose.position_as_list(), pose.position_as_list()) - target_x, target_y = position_diff[0], position_diff[1] - target_angle = self.get_z_angle_difference(robot_obj.original_pose.orientation_as_list(), - pose.orientation_as_list()) - # Get the joints of the base link - move_base_joints = self.get_move_base_joints() - return {move_base_joints.translation_x: target_x, - move_base_joints.translation_y: target_y, - move_base_joints.angular_z: target_angle} - - def get_z_angle_difference(self, original_orientation: List[float], target_orientation: List[float]) -> float: - """ - Get the difference between two z angles. - param original_orientation: The original orientation. - param target_orientation: The target orientation. - return: The difference between the two z angles. - """ - # rotation_quaternion = quaternion_from_euler(0, 0, np.pi / 4) - # new_quaternion = quaternion_multiply(original_orientation, rotation_quaternion) - return self.get_z_angle(target_orientation) - self.get_z_angle(original_orientation) - - def get_move_base_joints(self) -> VirtualMoveBaseJoints: + def get_robot_move_base_joints(self) -> VirtualMoveBaseJoints: """ Get the move base joints of the robot. @@ -665,36 +627,17 @@ def get_move_base_joints(self) -> VirtualMoveBaseJoints: """ return self.robot_description.virtual_move_base_joints - @staticmethod - def get_position_diff(current_position: List[float], target_position: List[float]) -> List[float]: - """ - Get the difference between two positions. - param current_position: The current position. - param target_position: The target position. - return: The difference between the two positions. - """ - return [target_position[i] - current_position[i] for i in range(3)] - - @staticmethod - def get_z_angle(target_quaternion: List[float]) -> float: - """ - Get the z angle from a quaternion by converting it to euler angles. - param target_quaternion: The target quaternion. - return: The z angle. - """ - return euler_from_quaternion(target_quaternion)[2] - @abstractmethod def perform_collision_detection(self) -> None: """ - Checks for collisions between all objects in the World and updates the contact points. + Check for collisions between all objects in the World and updates the contact points. """ pass @abstractmethod def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ - Returns a list of contact points of this Object with all other Objects. + Return a list of contact points of this Object with all other Objects. :param obj: The object. :return: A list of all contact points with other objects diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index a2f201bd8..22690871b 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -6,6 +6,7 @@ import numpy as np import rospy from geometry_msgs.msg import Point, Quaternion +from tf.transformations import euler_from_quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, @@ -95,10 +96,43 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + def set_mobile_robot_pose(self, pose: Pose) -> None: + """ + Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when + the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). + + :param pose: The target pose. + """ + goal = self.get_move_base_joint_goal(pose) + self.set_multiple_joint_positions(goal) + + def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + """ + Get the goal for the move base joints of a mobile robot to reach a target pose. + + :param pose: The target pose. + :return: The goal for the move base joints. + """ + target_translation, target_angle = self.get_mobile_base_pose_difference(pose) + # Get the joints of the base link + move_base_joints = self.world.get_robot_move_base_joints() + return {move_base_joints.translation_x: target_translation.x, + move_base_joints.translation_y: target_translation.y, + move_base_joints.angular_z: target_angle} + + def get_mobile_base_pose_difference(self, pose: Pose) -> Tuple[Point, float]: + """ + Get the difference between the current and the target pose of the mobile base. + + :param pose: The target pose. + :return: The difference between the current and the target pose of the mobile base. + """ + return self.original_pose.get_position_diff(pose), self.original_pose.get_z_angle_difference(pose) + @property def joint_actuators(self) -> Optional[Dict[str, str]]: """ - Returns the joint actuators of the robot. + The joint actuators of the robot. """ if self.obj_type == ObjectType.ROBOT: return self.robot_description.joint_actuators @@ -107,20 +141,21 @@ def joint_actuators(self) -> Optional[Dict[str, str]]: @property def has_actuators(self) -> bool: """ - Returns True if the object has actuators, otherwise False. + True if the object has actuators, otherwise False. """ return self.robot_description.has_actuators @property def robot_description(self) -> RobotDescription: """ - Returns the current robot description. + The current robot description. """ return self.world.robot_description def get_actuator_for_joint(self, joint: Joint) -> Optional[str]: """ Get the actuator name for a joint. + :param joint: The joint object for which to get the actuator. :return: The name of the actuator. """ @@ -129,38 +164,43 @@ def get_actuator_for_joint(self, joint: Joint) -> Optional[str]: def get_multiple_link_positions(self, links: List[Link]) -> Dict[str, List[float]]: """ Get the positions of multiple links of the object. - param link_names: The names of the links. - return: The positions of the links. + + :param links: The link objects of which to get the positions. + :return: The positions of the links. """ return self.world.get_multiple_link_positions(links) def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[float]]: """ Get the orientations of multiple links of the object. - param link_names: The names of the links. - return: The orientations of the links. + + :param links: The link objects of which to get the orientations. + :return: The orientations of the links. """ return self.world.get_multiple_link_orientations(links) def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: """ Get the poses of multiple links of the object. - param link_names: The names of the links. - return: The poses of the links. + + :param links: The link objects of which to get the poses. + :return: The poses of the links. """ return self.world.get_multiple_link_poses(links) def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the target poses of the attached objects. - return: The target poses of the attached objects + + :return: The target poses of the attached objects """ return self.get_target_poses_of_attached_objects_given_parent(self.get_pose()) def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the poses of the attached objects. - return: The poses of the attached objects + + :return: The poses of the attached objects """ return {child_object: attachment.get_child_object_pose() for child_object, attachment in self.attachments.items() if not attachment.loose} @@ -169,31 +209,47 @@ def get_target_poses_of_attached_objects_given_parent(self, pose: Pose) -> Dict[ """ Get the target poses of the attached objects of an object. Given the pose of the parent object. param pose: The pose of the parent object. - return: The target poses of the attached objects + + :return: The target poses of the attached objects """ return {child_object: attachment.get_child_object_pose_given_parent(pose) for child_object, attachment in self.attachments.items() if not attachment.loose} @property def name(self): + """ + The name of the object. + """ return self._name @name.setter def name(self, name: str): + """ + Set the name of the object. + """ self._name = name if name in [obj.name for obj in self.world.objects]: raise ObjectAlreadyExists(self) @property def pose(self): + """ + The current pose of the object. + """ return self.get_pose() @pose.setter def pose(self, pose: Pose): + """ + Set the pose of the object. + """ self.set_pose(pose) @property def transform(self): + """ + The current transform of the object. + """ return self.get_pose().to_transform(self.tf_frame) def _load_object_and_get_id(self, path: str, @@ -290,34 +346,43 @@ def _init_joints(self): self.joints[joint_name] = self.description.Joint(joint_id, parsed_joint_description, self, is_virtual) def is_joint_virtual(self, name: str): + """ + Check if a joint is virtual. + """ return self.description.is_joint_virtual(name) @property def virtual_joint_names(self): + """ + The names of the virtual joints. + """ return self.description.virtual_joint_names @property def virtual_joints(self): + """ + The virtual joints as a list. + """ return [joint for joint in self.joints.values() if joint.is_virtual] @property def has_one_link(self) -> bool: """ - Return True if the object has only one link, otherwise False. + True if the object has only one link, otherwise False. """ return len(self.links) == 1 @property def link_names(self) -> List[str]: """ - :return: The name of each link as a list. + The names of the links as a list. """ return self.world.get_object_link_names(self) @property def joint_names(self) -> List[str]: """ - :return: The name of each joint as a list. + The names of the joints as a list. """ return self.world.get_object_joint_names(self) @@ -497,6 +562,7 @@ def reset(self, remove_saved_states=True) -> None: def has_type_environment(self) -> bool: """ Check if the object is of type environment. + :return: True if the object is of type environment, False otherwise. """ return self.obj_type == ObjectType.ENVIRONMENT @@ -706,6 +772,7 @@ def current_state(self, state: ObjectState) -> None: def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Set the attachments of this object to the given attachments. + :param attachments: A dictionary with the object as key and the attachment as value. """ self.detach_objects_not_in_attachments(attachments) @@ -714,6 +781,7 @@ def set_attachments(self, attachments: Dict[Object, Attachment]) -> None: def detach_objects_not_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Detach objects that are not in the attachments list and are in the current attachments list. + :param attachments: A dictionary with the object as key and the attachment as value. """ copy_of_attachments = self.attachments.copy() @@ -731,6 +799,7 @@ def detach_objects_not_in_attachments(self, attachments: Dict[Object, Attachment def attach_objects_in_attachments(self, attachments: Dict[Object, Attachment]) -> None: """ Attach objects that are in the given attachments list but not in the current attachments list. + :param attachments: A dictionary with the object as key and the attachment as value. """ for obj, attachment in attachments.items(): @@ -750,6 +819,7 @@ def attach_objects_in_attachments(self, attachments: Dict[Object, Attachment]) - def mimic_attachment_with_object(self, attachment: Attachment, child_object: Object) -> None: """ Mimic the given attachment for this and the given child objects. + :param attachment: The attachment to mimic. :param child_object: The child object. """ @@ -764,8 +834,9 @@ def mimic_attachment_with_object(self, attachment: Attachment, child_object: Obj def get_attachment_transform_with_object(self, attachment: Attachment, child_object: Object) -> Transform: """ - Returns the attachment transform for the given parent and child objects, taking into account the prospection + Return the attachment transform for the given parent and child objects, taking into account the prospection world. + :param attachment: The attachment. :param child_object: The child object. :return: The attachment transform. @@ -1006,7 +1077,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ - Sets the current position of multiple joints at once, this method should be preferred when setting + Set the current position of multiple joints at once, this method should be preferred when setting multiple joints at once instead of running :func:`~Object.set_joint_position` in a loop. :param joint_positions: A dictionary with the joint names as keys and the target positions as values. @@ -1025,6 +1096,7 @@ def _update_on_joint_position_change(self): def get_joint_position(self, joint_name: str) -> float: """ Return the current position of the given joint. + :param joint_name: The name of the joint :return: The current position of the given joint """ @@ -1033,6 +1105,7 @@ def get_joint_position(self, joint_name: str) -> float: def get_joint_damping(self, joint_name: str) -> float: """ Return the damping of the given joint (friction). + :param joint_name: The name of the joint :return: The damping of the given joint """ @@ -1041,6 +1114,7 @@ def get_joint_damping(self, joint_name: str) -> float: def get_joint_upper_limit(self, joint_name: str) -> float: """ Return the upper limit of the given joint. + :param joint_name: The name of the joint :return: The upper limit of the given joint """ @@ -1049,6 +1123,7 @@ def get_joint_upper_limit(self, joint_name: str) -> float: def get_joint_lower_limit(self, joint_name: str) -> float: """ Return the lower limit of the given joint. + :param joint_name: The name of the joint :return: The lower limit of the given joint """ @@ -1057,6 +1132,7 @@ def get_joint_lower_limit(self, joint_name: str) -> float: def get_joint_axis(self, joint_name: str) -> Point: """ Return the axis of the given joint. + :param joint_name: The name of the joint :return: The axis of the given joint """ @@ -1065,6 +1141,7 @@ def get_joint_axis(self, joint_name: str) -> Point: def get_joint_type(self, joint_name: str) -> JointType: """ Return the type of the given joint. + :param joint_name: The name of the joint :return: The type of the given joint """ @@ -1073,6 +1150,7 @@ def get_joint_type(self, joint_name: str) -> JointType: def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: """ Return the lower and upper limits of the given joint. + :param joint_name: The name of the joint :return: The lower and upper limits of the given joint """ @@ -1081,6 +1159,7 @@ def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: """ Return the child link of the given joint. + :param joint_name: The name of the joint :return: The child link of the given joint """ @@ -1088,7 +1167,7 @@ def get_joint_child_link(self, joint_name: str) -> ObjectDescription.Link: def get_joint_parent_link(self, joint_name: str) -> ObjectDescription.Link: """ - + Return the parent link of the given joint. :param joint_name: The name of the joint :return: The parent link of the given joint @@ -1116,7 +1195,7 @@ def find_joint_above_link(self, link_name: str, joint_type: JointType) -> str: def get_multiple_joint_positions(self, joint_names: List[str]) -> Dict[str, float]: """ - Returns the positions of multiple joints at once. + Return the positions of multiple joints at once. :param joint_names: A list of joint names. :return: A dictionary with the joint names as keys and the joint positions as values. @@ -1134,6 +1213,8 @@ def get_positions_of_all_joints(self) -> Dict[str, float]: def update_link_transforms(self, transform_time: Optional[rospy.Time] = None) -> None: """ Update the transforms of all links of this object using time 'transform_time' or the current ros time. + + :param transform_time: The time to use for the transform update. """ for link in self.links.values(): link.update_transform(transform_time) @@ -1222,6 +1303,7 @@ def links_colors(self) -> Dict[str, Color]: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ Return the axis aligned bounding box of this object. + :return: The axis aligned bounding box of this object. """ return self.world.get_object_axis_aligned_bounding_box(self) @@ -1229,6 +1311,7 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: def get_base_origin(self) -> Pose: """ Return the origin of the base/bottom of this object. + :return: the origin of the base/bottom of this object. """ aabb = self.get_axis_aligned_bounding_box() @@ -1256,7 +1339,7 @@ def copy_to_prospection(self) -> Object: def copy_to_world(self, world: World) -> Object: """ - Copies this object to the given world. + Copy this object to the given world. :param world: The world to which the object should be copied. :return: The copied object in the given world. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 387a08d0e..2a1b651d0 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -369,7 +369,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): - self.set_mobile_robot_pose(obj, pose) + obj.set_mobile_robot_pose(pose) else: self._set_body_pose(obj.name, pose) @@ -383,7 +383,7 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None for obj in objects.keys(): if (obj.obj_type == ObjectType.ROBOT and RobotDescription.current_robot_description.virtual_move_base_joints is not None): - self.set_mobile_robot_pose(obj, objects[obj]) + obj.set_mobile_robot_pose(objects[obj]) objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, ObjectType.ROBOT]} self._set_multiple_body_poses({obj.name: pose for obj, pose in objects.items()}) From 082bffd0dd6331782eb6a77868406f27e637b4a4 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 14 Aug 2024 23:22:24 +0200 Subject: [PATCH 137/189] [MultiverseAction] removed extra line. --- src/pycram/designators/location_designator.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index d59f60a30..c3462bc56 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -249,7 +249,6 @@ def __iter__(self) -> Location: final_map = occupancy + gaussian - test_robot = World.current_world.get_prospection_object_for_object(self.robot) # Find a Joint of type prismatic which is above the handle in the URDF tree @@ -283,8 +282,10 @@ def __iter__(self) -> Location: valid_goal, arms_goal = reachability_validator(maybe_pose, test_robot, goal_pose, allowed_collision={test_robot: hand_links}) - if valid_init and valid_goal: - yield self.Location(maybe_pose, list(set(arms_init).intersection(set(arms_goal)))) + arms_list = list(set(arms_init).intersection(set(arms_goal))) + + if valid_init and valid_goal and len(arms_list) > 0: + yield self.Location(maybe_pose, arms_list) class SemanticCostmapLocation(LocationDesignatorDescription): From 51ad55a6507c7dc1146f343d0d58c3b8e6d2b341 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Aug 2024 13:43:00 +0200 Subject: [PATCH 138/189] [MultiverseAction] Added validate multi pose goals decorator. Added a DEBUG configuration mode int the world_conf.py, this allows for example plotting intermediate detection results. --- demos/pycram_bullet_world_demo/demo.py | 40 ++++++++------ demos/pycram_multiverse_demo/demo.py | 63 ++++++++++++++++++++--- src/pycram/config/world_conf.py | 5 ++ src/pycram/datastructures/world.py | 3 +- src/pycram/utils.py | 9 +++- src/pycram/validation/goal_validator.py | 28 +++++++--- src/pycram/world_concepts/world_object.py | 3 +- src/pycram/world_reasoning.py | 31 +++++++---- src/pycram/worlds/bullet_world.py | 4 +- src/pycram/worlds/multiverse.py | 3 +- 10 files changed, 144 insertions(+), 45 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index a60df7771..eb7f6c5c0 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -8,10 +8,13 @@ from pycram.object_descriptors.urdf import ObjectDescription from pycram.world_concepts.world_object import Object from pycram.datastructures.dataclasses import Color +from pycram.ros.viz_marker_publisher import VizMarkerPublisher extension = ObjectDescription.get_file_extension() -world = BulletWorld(WorldMode.GUI) +world = BulletWorld(WorldMode.DIRECT) +viz_marker_publisher = VizMarkerPublisher() + robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") @@ -43,22 +46,22 @@ def move_and_detect(obj_type): with simulated_robot: - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.25]).resolve().perform() - - milk_desig = move_and_detect(ObjectType.MILK) - - TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() - - cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) - - TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() - - bowl_desig = move_and_detect(ObjectType.BOWL) - - TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() - + # ParkArmsAction([Arms.BOTH]).resolve().perform() + # + # MoveTorsoAction([0.25]).resolve().perform() + # + # milk_desig = move_and_detect(ObjectType.MILK) + # + # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + # + # cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) + # + # TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() + # + # bowl_desig = move_and_detect(ObjectType.BOWL) + # + # TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + # # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), @@ -94,3 +97,6 @@ def move_and_detect(obj_type): PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() + +viz_marker_publisher._stop_publishing() +world.exit() diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index 814d08490..e8e00ff33 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -1,9 +1,10 @@ from pycram.datastructures.dataclasses import Color -from pycram.datastructures.enums import ObjectType, Arms +from pycram.datastructures.enums import ObjectType, Arms, Grasp from pycram.datastructures.pose import Pose from pycram.designators.action_designator import ParkArmsAction, MoveTorsoAction, TransportAction, NavigateAction, \ - LookAtAction, DetectAction -from pycram.designators.object_designator import BelieveObject + LookAtAction, DetectAction, OpenAction, PickUpAction, CloseAction, PlaceAction +from pycram.designators.location_designator import CostmapLocation, AccessingLocation +from pycram.designators.object_designator import BelieveObject, ObjectPart from pycram.object_descriptors.urdf import ObjectDescription from pycram.process_module import simulated_robot, with_simulated_robot from pycram.world_concepts.world_object import Object @@ -18,6 +19,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() + return object_desig @@ -29,16 +31,65 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) -pick_pose = Pose([2.6, 2.15, 1]) +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), + color=Color(0, 0, 1, 1)) +apartment.attach(spoon, 'cabinet10_drawer1') + +robot_desig = BelieveObject(names=[robot.name]) +apartment_desig = BelieveObject(names=[apartment.name]) + with simulated_robot: + + # Transport the milk ParkArmsAction([Arms.BOTH]).resolve().perform() MoveTorsoAction([0.25]).resolve().perform() - milk_desig = move_and_detect(ObjectType.MILK, pick_pose) + # NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + # + # LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() + # + # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + # + # TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + + + # Find and navigate to the drawer containing the spoon + handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve()) + drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), + robot_desig=robot_desig.resolve()).resolve() + + NavigateAction([drawer_open_loc.pose]).resolve().perform() + + OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + # spoon.detach(apartment) + + # Detect and pickup the spoon + LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform() + + spoon_desig = DetectAction(BelieveObject(types=[ObjectType.SPOON])).resolve().perform() - TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT + PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() + + ParkArmsAction([Arms.LEFT if pickup_arm == Arms.LEFT else Arms.RIGHT]).resolve().perform() + + CloseAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.15]).resolve().perform() + + # Find a pose to place the spoon, move and then place it + spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1]) + placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() + + NavigateAction([placing_loc.pose]).resolve().perform() + + PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() world.reset_world_and_remove_objects() world.exit() diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index 1e2e9181a..8570b20c3 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -25,6 +25,11 @@ Whether to update the poses from the simulator when getting the object poses. """ +DEBUG: bool = False +""" +Whether to use in debug mode. (This is used to print debug messages, plot images, etc.) +""" + @dataclass class JobHandling: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 679886726..1a383d73b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -30,7 +30,7 @@ PoseGoalValidator, JointPositionGoalValidator, MultiJointPositionGoalValidator, GoalValidator, validate_joint_position, validate_multiple_joint_positions, - validate_object_pose) + validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint from ..world_concepts.event import Event @@ -726,6 +726,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ pass + @validate_multiple_object_poses @abstractmethod def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: """ diff --git a/src/pycram/utils.py b/src/pycram/utils.py index 9397b3df6..eb0b6fb28 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -334,12 +334,17 @@ def construct_grid_of_camera_rays_angles(camera_description: 'CameraDescription' rays_vertical_angles = np.tile(rays_vertical_angles, (size, 1)).T return rays_horizontal_angles, rays_vertical_angles - def plot_segmentation_mask(self, segmentation_mask): + @staticmethod + def plot_segmentation_mask(segmentation_mask, + object_id_to_name: Dict[int, str] = None): """ Plot the segmentation mask with different colors for each object. :param segmentation_mask: The segmentation mask. + :param object_id_to_name: The mapping from object id to object name. """ + if object_id_to_name is None: + object_id_to_name = {uid: str(uid) for uid in np.unique(segmentation_mask)} # Create a custom color map unique_ids = np.unique(segmentation_mask) @@ -369,7 +374,7 @@ def plot_segmentation_mask(self, segmentation_mask): # Create color bar cbar = fig.colorbar(plt.cm.ScalarMappable(norm=norm, cmap=cmap), ax=ax, ticks=np.arange(len(unique_ids))) cbar.ax.set_yticklabels( - [self.object_id_to_name[uid] for uid in unique_ids]) # Label the color bar with object IDs + [object_id_to_name[uid] for uid in unique_ids]) # Label the color bar with object IDs cbar.set_label('Object Name') plt.show() diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 9f2d4e8f0..7e3b57334 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -447,19 +447,33 @@ def validate_object_pose(pose_setter_func): def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): - attachments_pose_goal = obj.get_target_poses_of_attached_objects_given_parent(pose) - if len(attachments_pose_goal) == 0: - world.pose_goal_validator.register_goal(pose, obj) - else: - world.multi_pose_goal_validator.register_goal([pose, *list(attachments_pose_goal.values())], - [obj, *list(attachments_pose_goal.keys())]) + world.pose_goal_validator.register_goal(pose, obj) if not pose_setter_func(world, obj, pose): world.pose_goal_validator.reset() - world.multi_pose_goal_validator.reset() return False world.pose_goal_validator.wait_until_goal_is_achieved() + return True + + return wrapper + + +def validate_multiple_object_poses(pose_setter_func): + """ + A decorator to validate multiple object poses. + :param pose_setter_func: The function to set multiple poses of the objects. + """ + + def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']): + + world.multi_pose_goal_validator.register_goal(list(object_poses.values()), + list(object_poses.keys())) + + if not pose_setter_func(world, object_poses): + world.multi_pose_goal_validator.reset() + return False + world.multi_pose_goal_validator.wait_until_goal_is_achieved() return True diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 22690871b..57d4c5236 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -1073,7 +1073,8 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: :param joint_name: The name of the joint :param joint_position: The target pose for this joint """ - self.world.reset_joint_position(self.joints[joint_name], joint_position) + if self.world.reset_joint_position(self.joints[joint_name], joint_position): + self._update_on_joint_position_change() def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index c65e2dc4e..0356c297b 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -6,7 +6,9 @@ from .datastructures.world import World, UseProspectionWorld from .external_interfaces.ik import try_to_reach, try_to_reach_with_grasp from .robot_description import RobotDescription +from .utils import RayTestUtils from .world_concepts.world_object import Object +from .config import world_conf as conf def stable(obj: Object) -> bool: @@ -62,24 +64,31 @@ def contact( def get_visible_objects( camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> Tuple[np.ndarray, Pose]: + front_facing_axis: Optional[List[float]] = None, + plot_segmentation_mask: bool = conf.DEBUG) -> Tuple[np.ndarray, Pose]: """ - Returns a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. + Return a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. :param camera_pose: The pose of the camera in world coordinate frame. :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param plot_segmentation_mask: If the segmentation mask should be plotted :return: A segmentation mask of the objects that are visible and the pose of the point at exactly 2 meters in front of the camera in the direction of the front facing axis with respect to the world coordinate frame. """ - front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis + if front_facing_axis is None: + front_facing_axis = RobotDescription.current_robot_description.get_default_camera().front_facing_axis - world_to_cam = camera_pose.to_transform("camera") + camera_frame = RobotDescription.current_robot_description.get_camera_frame() + world_to_cam = camera_pose.to_transform(camera_frame) - cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], "camera", + cam_to_point = Transform(list(np.multiply(front_facing_axis, 2)), [0, 0, 0, 1], camera_frame, "point") target_point = (world_to_cam * cam_to_point).to_pose() seg_mask = World.current_world.get_images_for_target(target_point, camera_pose)[2] + if plot_segmentation_mask: + RayTestUtils.plot_segmentation_mask(seg_mask) + return seg_mask, target_point @@ -87,7 +96,8 @@ def visible( obj: Object, camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - threshold: float = 0.8) -> bool: + threshold: float = 0.8, + plot_segmentation_mask: bool = conf.DEBUG) -> bool: """ Checks if an object is visible from a given position. This will be achieved by rendering the object alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the @@ -97,6 +107,7 @@ def visible( :param camera_pose: The pose of the camera in map frame :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz :param threshold: The minimum percentage of the object that needs to be visible for this method to return true. + :param plot_segmentation_mask: If the segmentation mask should be plotted. :return: True if the object is visible from the camera_position False if not """ with UseProspectionWorld(): @@ -111,7 +122,7 @@ def visible( else: obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1]), set_attachments=False) - seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis, plot_segmentation_mask) max_pixel = np.array(seg_mask == prospection_obj.id).sum() World.current_world.restore_state(state_id) @@ -129,7 +140,8 @@ def visible( def occluding( obj: Object, camera_pose: Pose, - front_facing_axis: Optional[List[float]] = None) -> List[Object]: + front_facing_axis: Optional[List[float]] = None, + plot_segmentation_mask: bool = conf.DEBUG) -> List[Object]: """ Lists all objects which are occluding the given object. This works similar to 'visible'. First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. @@ -139,6 +151,7 @@ def occluding( :param obj: The object for which occlusion should be checked :param camera_pose: The pose of the camera in world coordinate frame :param front_facing_axis: The axis, of the camera frame, which faces to the front of the robot. Given as list of xyz + :param plot_segmentation_mask: If the segmentation mask should be plotted :return: A list of occluding objects """ @@ -152,7 +165,7 @@ def occluding( else: other_obj.set_pose(Pose([100, 100, 0], [0, 0, 0, 1])) - seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis) + seg_mask, target_point = get_visible_objects(camera_pose, front_facing_axis, plot_segmentation_mask) # All indices where the object that could be occluded is in the image # [0] at the end is to reduce by one dimension because dstack adds an unnecessary dimension diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index ded66cbb1..8fc779f60 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -17,7 +17,8 @@ from ..datastructures.pose import Pose from ..object_descriptors.urdf import ObjectDescription from ..datastructures.world import World -from ..validation.goal_validator import validate_multiple_joint_positions, validate_joint_position, validate_object_pose +from ..validation.goal_validator import (validate_multiple_joint_positions, validate_joint_position, + validate_object_pose, validate_multiple_object_poses) from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object @@ -198,6 +199,7 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} + @validate_multiple_object_poses def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 2a1b651d0..7ebaa4a2e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -20,7 +20,7 @@ from ..description import Link, Joint from ..robot_description import RobotDescription from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ - validate_joint_position + validate_joint_position, validate_multiple_object_poses from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object from ..utils import RayTestUtils @@ -375,6 +375,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: return True + @validate_multiple_object_poses def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: """ Reset the poses of multiple objects in the simulator. From 0308f4a2e39dd5a5e97ff5bf742baf4d2623cb5f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Aug 2024 14:03:32 +0200 Subject: [PATCH 139/189] [MultiverseAction] test get and spawn of environment type objects. --- test/test_multiverse.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index a9392bd7f..364f75a4d 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -227,6 +227,11 @@ def step_robot_pose(self, robot, position_step, angle_step, num_steps): self.assert_poses_are_equal(new_pose, robot_pose, position_delta=self.multiverse.acceptable_position_error, orientation_delta=self.multiverse.acceptable_orientation_error) + def test_get_environment_pose(self): + apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") + pose = apartment.get_pose() + self.assertIsInstance(pose, Pose) + def test_attach_object(self): milk = self.spawn_milk([1, 0.1, 0.1]) cup = self.spawn_cup([1, 1.1, 0.1]) From 0753dd81375101a7c372d7e579a8745e97cc2958 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Aug 2024 19:14:57 +0200 Subject: [PATCH 140/189] [MultiverseAction] Demo is working. --- demos/pycram_multiverse_demo/demo.py | 30 ++++++++++++++++--------- src/pycram/validation/goal_validator.py | 2 +- src/pycram/worlds/bullet_world.py | 3 ++- src/pycram/worlds/multiverse.py | 9 +++++--- 4 files changed, 28 insertions(+), 16 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index e8e00ff33..857d60d66 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -31,14 +31,13 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), +spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), color=Color(0, 0, 1, 1)) apartment.attach(spoon, 'cabinet10_drawer1') robot_desig = BelieveObject(names=[robot.name]) apartment_desig = BelieveObject(names=[apartment.name]) - with simulated_robot: # Transport the milk @@ -46,14 +45,13 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): MoveTorsoAction([0.25]).resolve().perform() - # NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() - # - # LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() - # - # milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() - # - # TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() + NavigateAction(target_locations=[Pose([1.7, 2, 0])]).resolve().perform() + + LookAtAction(targets=[Pose([2.6, 2.15, 1])]).resolve().perform() + milk_desig = DetectAction(BelieveObject(types=[milk.obj_type])).resolve().perform() + + TransportAction(milk_desig, [Arms.LEFT], [Pose([2.4, 3, 1.02])]).resolve().perform() # Find and navigate to the drawer containing the spoon handle_desig = ObjectPart(names=["cabinet10_drawer1_handle"], part_of=apartment_desig.resolve()) @@ -63,7 +61,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): NavigateAction([drawer_open_loc.pose]).resolve().perform() OpenAction(object_designator_description=handle_desig, arms=[drawer_open_loc.arms[0]]).resolve().perform() - # spoon.detach(apartment) + spoon.detach(apartment) # Detect and pickup the spoon LookAtAction([apartment.get_link_pose("cabinet10_drawer1_handle")]).resolve().perform() @@ -82,7 +80,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): MoveTorsoAction([0.15]).resolve().perform() # Find a pose to place the spoon, move and then place it - spoon_target_pose = Pose([4.85, 3.3, 0.8], [0, 0, 1, 1]) + spoon_target_pose = Pose([2.35, 2.6, 0.95], [0, 0, 0, 1]) placing_loc = CostmapLocation(target=spoon_target_pose, reachable_for=robot_desig.resolve()).resolve() NavigateAction([placing_loc.pose]).resolve().perform() @@ -93,3 +91,13 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): world.reset_world_and_remove_objects() world.exit() + + +def debug_place_spoon(): + robot.set_pose(Pose([1.66, 2.56, 0.01], [0.0, 0.0, -0.04044101807153309, 0.9991819274072855])) + spoon.set_pose(Pose([1.9601757566599975, 2.06718019258626, 1.0427727691273496], + [0.11157527804553227, -0.7076243776942466, 0.12773644958149588, 0.685931554070963])) + robot.attach(spoon, 'r_gripper_tool_frame') + pickup_arm = Arms.RIGHT + spoon_desig = BelieveObject(names=[spoon.name]) + return pickup_arm, spoon_desig diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 7e3b57334..1a3d40a02 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -136,7 +136,7 @@ def register_goal(self, goal_value: Any, :param initial_value: The initial value. :param acceptable_error: The acceptable error. """ - if goal_value is None or hasattr(goal_value, '__len__') and len(goal_value) == 0: + if goal_value is None or (hasattr(goal_value, '__len__') and len(goal_value) == 0): return # Skip if goal value is None or empty self.goal_value = goal_value self.current_value_getter_input = current_value_getter_input diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 8fc779f60..ca4de1fe4 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -200,9 +200,10 @@ def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: return {joint.name: self.get_joint_position(joint) for joint in joints} @validate_multiple_object_poses - def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: + def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool: for obj, pose in objects.items(): self.reset_object_base_pose(obj, pose) + return True @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 7ebaa4a2e..667ff2fb5 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -360,7 +360,11 @@ def get_object_pose(self, obj: Object) -> Pose: return self._get_body_pose(obj.name) def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: - return self._get_multiple_body_poses([obj.name for obj in objects]) + env_objects = [obj for obj in objects if obj.obj_type == ObjectType.ENVIRONMENT] + non_env_objects = [obj for obj in objects if obj.obj_type != ObjectType.ENVIRONMENT] + all_poses = self._get_multiple_body_poses([obj.name for obj in non_env_objects]) + all_poses.update({obj.name: Pose() for obj in env_objects}) + return all_poses @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: @@ -575,7 +579,7 @@ def ray_test_batch(self, from_positions: List[List[float]], if ray_result.intersected(): body_name = ray_result.body_name if body_name == "world": - results[-1].append(self.floor.id) + results[-1].append(0) # The floor id, which is always 0 since the floor is spawned first. elif body_name in self.object_name_to_id.keys(): results[-1].append(self.object_name_to_id[body_name]) else: @@ -610,7 +614,6 @@ def remove_physics_simulator_state(self, state_id: int) -> None: def restore_physics_simulator_state(self, state_id: int) -> None: logging.error("restore_physics_simulator_state is not implemented in Multiverse") - raise NotImplementedError def set_link_color(self, link: Link, rgba_color: Color): logging.warning("set_link_color is not implemented in Multiverse") From b6c23723d9d91dc85732c77eda820ba5569349ec Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 15 Aug 2024 19:17:38 +0200 Subject: [PATCH 141/189] [MultiverseAction] Demo is working. --- demos/pycram_bullet_world_demo/demo.py | 32 +++++++++++++------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index eb7f6c5c0..eee4037c9 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -46,22 +46,22 @@ def move_and_detect(obj_type): with simulated_robot: - # ParkArmsAction([Arms.BOTH]).resolve().perform() - # - # MoveTorsoAction([0.25]).resolve().perform() - # - # milk_desig = move_and_detect(ObjectType.MILK) - # - # TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() - # - # cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) - # - # TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() - # - # bowl_desig = move_and_detect(ObjectType.BOWL) - # - # TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() - # + ParkArmsAction([Arms.BOTH]).resolve().perform() + + MoveTorsoAction([0.25]).resolve().perform() + + milk_desig = move_and_detect(ObjectType.MILK) + + TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + + cereal_desig = move_and_detect(ObjectType.BREAKFAST_CEREAL) + + TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform() + + bowl_desig = move_and_detect(ObjectType.BOWL) + + TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform() + # Finding and navigating to the drawer holding the spoon handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(), From 87ef6d2faa9597e7788ced9ebbb001ad58329f8e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Aug 2024 10:43:39 +0200 Subject: [PATCH 142/189] [MultiverseAction] Added a step that removes all objects and resets the world when exiting the world. --- demos/pycram_bullet_world_demo/demo.py | 4 +--- demos/pycram_multiverse_demo/demo.py | 1 - src/pycram/datastructures/world.py | 1 + 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index eee4037c9..f06fc3d38 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -12,8 +12,7 @@ extension = ObjectDescription.get_file_extension() -world = BulletWorld(WorldMode.DIRECT) -viz_marker_publisher = VizMarkerPublisher() +world = BulletWorld(WorldMode.GUI) robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", pose=Pose([1, 2, 0])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") @@ -98,5 +97,4 @@ def move_and_detect(obj_type): ParkArmsAction([Arms.BOTH]).resolve().perform() -viz_marker_publisher._stop_publishing() world.exit() diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index 857d60d66..cb5b36508 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -89,7 +89,6 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): ParkArmsAction([Arms.BOTH]).resolve().perform() -world.reset_world_and_remove_objects() world.exit() diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 1a383d73b..6a0291044 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -856,6 +856,7 @@ def exit(self) -> None: Closes the World as well as the prospection world, also collects any other thread that is running. """ self.exit_prospection_world_if_exists() + self.reset_world_and_remove_objects() self.disconnect_from_physics_server() self.reset_robot() self.join_threads() From 3f26492b80b1cc653296f0e059441bb5af69b0b3 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Aug 2024 15:28:13 +0200 Subject: [PATCH 143/189] [MultiverseAction] doc and cleaning --- src/pycram/description.py | 19 +++++ src/pycram/world_concepts/world_object.py | 1 - src/pycram/worlds/multiverse.py | 86 +++++++++++++++-------- 3 files changed, 74 insertions(+), 32 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 06dcdd491..eae00eae2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -219,20 +219,39 @@ def set_pose(self, pose: Pose) -> None: Set the pose of this link to the given pose. NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints that can allow the link to be at the given pose. + :param pose: The target pose for this link. """ self.object.set_pose(self.get_object_pose_given_link_pose(pose)) def get_object_pose_given_link_pose(self, pose): + """ + Get the object pose given the link pose, which could be a hypothetical link pose to see what would be the object + pose in that case (assuming that the object itself moved not the joints). + + :param pose: The link pose. + """ return (pose.to_transform(self.tf_frame) * self.get_transform_to_root_link()).to_pose() def get_pose_given_object_pose(self, pose): + """ + Get the link pose given the object pose, which could be a hypothetical object pose to see what would be the link + pose in that case (assuming that the object itself moved not the joints). + + :param pose: The object pose. + """ return (pose.to_transform(self.object.tf_frame) * self.get_transform_from_root_link()).to_pose() def get_transform_from_root_link(self) -> Transform: + """ + Return the transformation from the root link of the object to this link. + """ return self.get_transform_from_link(self.object.root_link) def get_transform_to_root_link(self) -> Transform: + """ + Return the transformation from this link to the root link of the object. + """ return self.get_transform_to_link(self.object.root_link) @property diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 57d4c5236..756bd1914 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -6,7 +6,6 @@ import numpy as np import rospy from geometry_msgs.msg import Point, Quaternion -from tf.transformations import euler_from_quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 667ff2fb5..5740195fb 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -66,8 +66,6 @@ class Multiverse(World): The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for the time taken by the simulation to process the request, this depends on the computational power of the machine running the simulation. - TODO: This should be replaced by a feedback mechanism that waits until a certain condition is met, e.g. the action - is completed. """ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, @@ -76,11 +74,11 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, simulation: Optional[str] = None): """ Initialize the Multiverse Socket and the PyCram World. - param mode: The mode of the world (DIRECT or GUI). - param is_prospection: Whether the world is prospection or not. - param simulation_frequency: The frequency of the simulation. - param client_addr: The address of the multiverse client. - param simulation: The name of the simulation. + + :param mode: The mode of the world (DIRECT or GUI). + :param is_prospection: Whether the world is prospection or not. + :param simulation_frequency: The frequency of the simulation. + :param simulation: The name of the simulation. """ self._make_sure_multiverse_resources_are_added() @@ -171,6 +169,9 @@ def get_images_for_target(self, target_pose: Pose, camera_min_distance: float = 0.1, camera_max_distance: int = 3, plot: bool = False) -> List[np.ndarray]: + """ + Uses ray test to get the images for the target object. (target_pose is currently not used) + """ camera_description = RobotDescription.current_robot_description.get_default_camera() camera_frame = RobotDescription.current_robot_description.get_camera_frame() return self.ray_test_utils.get_images_for_target(cam_pose, camera_description, camera_frame, @@ -188,9 +189,9 @@ def get_joint_position_name(joint: Joint) -> MultiverseJointPosition: def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: """ Spawn the robot in the simulator. - param robot_description: The robot description. - param pose: The pose of the robot. - return: The object of the robot. + + :param name: The name of the robot. + :param pose: The pose of the robot. """ actuator_joint_commands = { actuator_name: [self.get_joint_cmd_name(self.robot_description.joint_types[joint_name]).value] @@ -207,9 +208,10 @@ def load_object_and_get_id(self, name: Optional[str] = None, """ Spawn the object in the simulator and return the object id. Object name has to be unique and has to be same as the name of the object in the description file. - param name: The name of the object to be loaded. - param pose: The pose of the object. - param obj_type: The type of the object. + + :param name: The name of the object to be loaded. + :param pose: The pose of the object. + :param obj_type: The type of the object. """ if pose is None: pose = Pose() @@ -223,10 +225,11 @@ def load_object_and_get_id(self, name: Optional[str] = None, def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: """ - Spawn the object in the simulator and return the object id. - param obj: The object to be spawned. - param pose: The pose of the object. - return: The object id. + Spawn the object in the simulator. + + :param name: The name of the object. + :param object_type: The type of the object. + :param pose: The pose of the object. """ if object_type == ObjectType.ROBOT and self.use_controller: self.spawn_robot_with_controller(name, pose) @@ -236,8 +239,9 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: def _update_object_id_name_maps_and_get_latest_id(self, name: str) -> int: """ Update the object id name maps and return the latest object id. - param name: The name of the object. - return: The latest object id. + + :param name: The name of the object. + :return: The latest object id. """ self.last_object_id += 1 self.object_name_to_id[name] = self.last_object_id @@ -271,6 +275,13 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: return True def _reset_joint_position_using_controller(self, joint: Joint, joint_position: float) -> bool: + """ + Reset the position of a joint in the simulator using the controller. + + :param joint: The joint. + :param joint_position: The position of the joint. + :return: True if the joint position is reset successfully. + """ self.joint_controller.set_body_property(self.get_actuator_for_joint(joint), self.get_joint_cmd_name(joint.type), [joint_position]) @@ -383,7 +394,8 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None: """ Reset the poses of multiple objects in the simulator. - param objects: The dictionary of objects and poses. + + :param objects: The dictionary of objects and poses. """ for obj in objects.keys(): if (obj.obj_type == ObjectType.ROBOT and @@ -396,15 +408,17 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None def _set_body_pose(self, body_name: str, pose: Pose) -> None: """ Reset the pose of a body (object, link, or joint) in the simulator. - param body_name: The name of the body. - param pose: The pose of the body. + + :param body_name: The name of the body. + :param pose: The pose of the body. """ self._set_multiple_body_poses({body_name: pose}) def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: """ Reset the poses of multiple bodies in the simulator. - param body_poses: The dictionary of body names and poses. + + :param body_poses: The dictionary of body names and poses. """ self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: @@ -415,9 +429,10 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optional[Pose]: """ Get the pose of a body in the simulator. - param body_name: The name of the body. + + :param body_name: The name of the body. :param wait: Whether to wait until the pose is received. - return: The pose of the body. + :return: The pose of the body. """ data = self.reader.get_body_pose(body_name, wait) return Pose(data[MultiverseBodyProperty.POSITION.value], @@ -497,8 +512,9 @@ def add_constraint(self, constraint: Constraint) -> int: def _update_constraint_collection_and_get_latest_id(self, constraint: Constraint) -> int: """ Update the constraint collection and return the latest constraint id. - param constraint: The constraint to be added. - return: The latest constraint id. + + :param constraint: The constraint to be added. + :return: The latest constraint id. """ self.last_constraint_id += 1 self.constraints[self.last_constraint_id] = constraint @@ -509,7 +525,7 @@ def remove_constraint(self, constraint_id) -> None: self.api_requester.detach(constraint) def perform_collision_detection(self) -> None: - pass + ... def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ @@ -534,8 +550,6 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: logging.error(f"Body link not found: {point.body_name}") raise ValueError(f"Body link not found: {point.body_name}") contact_points.append(ContactPoint(obj.root_link, body_link)) - # b_obj = body_link.object - # normal_force_in_b_frame = self._get_normal_force_on_object_from_contact_force(b_obj, point.contact_force) contact_points[-1].force_x_in_world_frame = point.contact_force[0] contact_points[-1].force_y_in_world_frame = point.contact_force[1] contact_points[-1].force_z_in_world_frame = point.contact_force[2] @@ -548,6 +562,10 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L """ Get the normal force on an object from the contact force exerted by another object that is expressed in the world frame. Thus transforming the contact force to the object frame is necessary. + + :param obj: The object. + :param contact_force: The contact force. + :return: The normal force on the object. """ obj_quat = obj.get_orientation_as_list() obj_rot_matrix = quaternion_matrix(obj_quat)[:3, :3] @@ -613,7 +631,7 @@ def remove_physics_simulator_state(self, state_id: int) -> None: logging.warning("remove_physics_simulator_state is not implemented in Multiverse") def restore_physics_simulator_state(self, state_id: int) -> None: - logging.error("restore_physics_simulator_state is not implemented in Multiverse") + logging.warning("restore_physics_simulator_state is not implemented in Multiverse") def set_link_color(self, link: Link, rgba_color: Color): logging.warning("set_link_color is not implemented in Multiverse") @@ -642,4 +660,10 @@ def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") def check_object_exists_in_multiverse(self, obj: Object) -> bool: + """ + Check if the object exists in the Multiverse world. + + :param obj: The object. + :return: True if the object exists, False otherwise. + """ return self.api_requester.check_object_exists(obj) From 1996f50dbb1465032e8228b3b45a62c93df14465 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Aug 2024 16:38:44 +0200 Subject: [PATCH 144/189] [MultiverseControl] merged with goal validator to update the latest changes. Fixed merge conflicts related to robot description and control mode. --- src/pycram/datastructures/world.py | 11 ++++++----- src/pycram/world_concepts/world_object.py | 24 ++++++++++------------- src/pycram/worlds/multiverse.py | 10 ++++++---- test/test_multiverse.py | 7 +++++-- 4 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a12fb1f97..7e7769429 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -36,7 +36,7 @@ if TYPE_CHECKING: from ..world_concepts.world_object import Object - from ..description import Link, Joint + from ..description import Link, Joint, ObjectDescription class World(StateEntity, ABC): @@ -291,16 +291,17 @@ def _sync_prospection_world(self): self.pause_world_sync() self.world_sync.start() - def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - obj: Object) -> str: + def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, + description: ObjectDescription, name: str) -> str: """ Updates the cache directory with the given object. :param path: The path to the object. :param ignore_cached_files: If the cached files should be ignored. - :param obj: The object to be added to the cache directory. + :param description: The object description. + :param name: The name of the object. """ - return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, obj.description, obj.name) + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name) @property def simulation_time_step(self): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 941360e4a..d21e6727d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -73,15 +73,15 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - self._update_world_robot_and_description() - - self.id, self.path = self._load_object_and_get_id(path, ignore_cached_files) + self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, + self.description, self.name) self.description.update_description_from_file(self.path) if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: - self._add_virtual_move_base_joints() + self._update_world_robot_and_description() + + self.id = self._spawn_object_and_get_id() self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name @@ -196,8 +196,7 @@ def pose(self, pose: Pose): def transform(self): return self.get_pose().to_transform(self.tf_frame) - def _load_object_and_get_id(self, path: str, - ignore_cached_files: bool = False) -> Tuple[int, Union[str, None]]: + def _spawn_object_and_get_id(self) -> int: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be used when an .obj or .stl file is given. @@ -207,16 +206,14 @@ def _load_object_and_get_id(self, path: str, This new file will have resolved mesh file paths, meaning there will be no references to ROS packages instead there will be absolute file paths. - :param path: The path to the description file. - :param ignore_cached_files: Whether to ignore files in the cache directory. :return: The unique id of the object and the path of the file that was loaded. """ - self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + + path = self.path if self.world.let_pycram_handle_spawning else self.name try: - path = self.path if self.world.let_pycram_handle_spawning else self.name obj_id = self.world.load_object_and_get_id(path, self._current_pose, self.obj_type) - return obj_id, self.path + return obj_id except Exception as e: logging.error( @@ -233,6 +230,7 @@ def _update_world_robot_and_description(self): rdm = RobotDescriptionManager() rdm.load_description(self.description.name) World.robot = self + self._add_virtual_move_base_joints() def _add_virtual_move_base_joints(self): """ @@ -936,7 +934,6 @@ def reset_all_joints_positions(self) -> None: joint_positions = [0] * len(joint_names) self.set_multiple_joint_positions(dict(zip(joint_names, joint_positions))) - def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ Set the position of the given joint to the given joint pose and updates the poses of all attached objects. @@ -946,7 +943,6 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: """ self.world.reset_joint_position(self.joints[joint_name], joint_position) - def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Sets the current position of multiple joints at once, this method should be preferred when setting diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 06d9e77fc..546290163 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -104,7 +104,8 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if not self.is_prospection_world: self._spawn_floor() - self.api_requester.pause_simulation() + if not self.use_controller: + self.api_requester.pause_simulation() def _init_clients(self): self.reader: MultiverseReader = self.client_manager.create_reader( @@ -517,9 +518,10 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L return results def step(self): - self.api_requester.unpause_simulation() - sleep(30 / self.simulation_frequency) - self.api_requester.pause_simulation() + if not self.use_controller: + self.api_requester.unpause_simulation() + sleep(30 / self.simulation_frequency) + self.api_requester.pause_simulation() def save_physics_simulator_state(self) -> int: logging.warning("save_physics_simulator_state is not implemented in Multiverse") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 5958b0c4e..831279a46 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -179,8 +179,11 @@ def test_set_joint_position(self): original_joint_position = robot.get_joint_position(joint) robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - delta = self.multiverse.acceptable_position_error if joint_type == JointType.PRISMATIC \ - else self.multiverse.acceptable_orientation_error + if not self.multiverse.use_controller: + delta = self.multiverse.acceptable_position_error if joint_type == JointType.PRISMATIC \ + else self.multiverse.acceptable_orientation_error + else: + delta = 0.18 self.assertAlmostEqual(joint_position, original_joint_position + step, delta=delta) def test_spawn_robot(self): From 284a1b843b0e0f55ece06de68f4170b6ed44ca48 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Aug 2024 16:57:51 +0200 Subject: [PATCH 145/189] [MultiverseAction] merged with multiverse_controller to update the latest changes. --- src/pycram/config/multiverse_conf.py | 6 +++--- src/pycram/worlds/multiverse.py | 2 +- test/test_multiverse.py | 6 ++++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 011ba58db..9b6c38750 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -15,8 +15,8 @@ """ # Multiverse Simulation Configuration -simulation_time_step: float = 1e-2 -simulation_frequency: int = int(1 / simulation_time_step) +simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) +simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) use_bullet_mode: bool = True """ @@ -24,7 +24,7 @@ similar to bullet_world which uses the bullet physics engine. """ -use_controller: bool = True +use_controller: bool = False """ Only used when use_bullet_mode is False. This turns on the controller for the robot joints. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 5740195fb..30f7e1772 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -53,7 +53,7 @@ class Multiverse(World): similar to bullet_world which uses the bullet physics engine. """ - use_controller: bool = conf.use_controller and not use_bullet_mode + use_controller: bool = conf.use_controller and not conf.use_bullet_mode """ Whether to use the controller for the robot joints or not. """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ad8d50ffd..4849e0b5f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -293,7 +293,9 @@ def test_get_object_contact_points(self): self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor) cup = self.spawn_cup([1, 1, 0.1]) - self.multiverse.simulate(0.2) + # 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) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -307,7 +309,7 @@ def test_get_contact_points_between_two_objects(self): cup = self.spawn_cup([1, 1, 0.1]) # 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.2) + self.multiverse.simulate(0.3) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From 5051b5a3a57998bb4b554884948862f2d2aef846 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 16 Aug 2024 20:50:27 +0200 Subject: [PATCH 146/189] [MultiverseMJCF] implemented link description for mjcf. --- src/pycram/datastructures/enums.py | 31 ++ src/pycram/object_descriptors/mjcf.py | 477 ++++++++++++++++++++++++++ 2 files changed, 508 insertions(+) create mode 100644 src/pycram/object_descriptors/mjcf.py diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index f1e95dba1..ea7fdc464 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -127,3 +127,34 @@ class VirtualMoveBaseJointName(Enum): LINEAR_X = "odom_vel_lin_x_joint" LINEAR_Y = "odom_vel_lin_y_joint" ANGULAR_Z = "odom_vel_ang_z_joint" + + +class MJCFGeomType(Enum): + """ + Enum for the different geom types in a MuJoCo XML file. + """ + BOX = "box" + CYLINDER = "cylinder" + CAPSULE = "capsule" + SPHERE = "sphere" + PLANE = "plane" + MESH = "mesh" + ELLIPSOID = "ellipsoid" + HFIELD = "hfield" + SDF = "sdf" + + +MJCFBodyType = MJCFGeomType +""" +Alias for MJCFGeomType. As the body type is the same as the geom type. +""" + + +class MJCFJointType(Enum): + """ + Enum for the different joint types in a MuJoCo XML file. + """ + FREE = "free" + BALL = "ball" + SLIDE = "slide" + HINGE = "hinge" diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py new file mode 100644 index 000000000..8537183aa --- /dev/null +++ b/src/pycram/object_descriptors/mjcf.py @@ -0,0 +1,477 @@ +import os +import pathlib +import xml.etree.ElementTree as ET + +import rospkg +import rospy +from geometry_msgs.msg import Point +from tf.transformations import quaternion_from_euler, euler_from_quaternion +from typing_extensions import Union, List, Optional, Dict, Tuple +from dm_control import mjcf + +from ..datastructures.enums import JointType, MJCFGeomType +from ..datastructures.pose import Pose +from ..description import JointDescription as AbstractJointDescription, \ + LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription +from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ + SphereVisualShape, MeshVisualShape +from ..failures import MultiplePossibleTipLinks +from ..utils import suppress_stdout_stderr + + +class LinkDescription(AbstractLinkDescription): + """ + A class that represents a link description of an object. + """ + + def __init__(self, mjcf_description: mjcf.Element): + super().__init__(mjcf_description) + + @property + def geometry(self) -> Union[VisualShape, None]: + """ + Returns the geometry type of the collision element of this link. + """ + return self._get_visual_shape(self.parsed_description.find_all('geom')[0]) + + @staticmethod + def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: + """ + Returns the VisualShape of the given URDF geometry. + """ + if mjcf_geometry.type == MJCFGeomType.BOX.value: + return BoxVisualShape(Color(), [0, 0, 0], mjcf_geometry.size) + if mjcf_geometry.type == MJCFGeomType.CYLINDER.value: + return CylinderVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0], mjcf_geometry.size[1]*2) + if mjcf_geometry.type == MJCFGeomType.SPHERE.value: + return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0]) + if mjcf_geometry.type == MJCFGeomType.MESH.value: + return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.scale, mjcf_geometry.filename) + return None + + @property + def origin(self) -> Union[Pose, None]: + """ + :return: The origin of this link. + """ + return parse_pose_from_body_element(self.parsed_description) + + @property + def name(self) -> str: + return self.parsed_description.name + + +class JointDescription(AbstractJointDescription): + urdf_type_map = {'unknown': JointType.UNKNOWN, + 'revolute': JointType.REVOLUTE, + 'continuous': JointType.CONTINUOUS, + 'prismatic': JointType.PRISMATIC, + 'floating': JointType.FLOATING, + 'planar': JointType.PLANAR, + 'fixed': JointType.FIXED} + + pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} + + def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False): + super().__init__(urdf_description, is_virtual=is_virtual) + + @property + def origin(self) -> Pose: + return Pose(self.parsed_description.origin.xyz, + quaternion_from_euler(*self.parsed_description.origin.rpy)) + + @property + def name(self) -> str: + return self.parsed_description.name + + @property + def has_limits(self) -> bool: + return bool(self.parsed_description.limit) + + @property + def type(self) -> JointType: + """ + :return: The type of this joint. + """ + return self.urdf_type_map[self.parsed_description.type] + + @property + def axis(self) -> Point: + """ + :return: The axis of this joint, for example the rotation axis for a revolute joint. + """ + return Point(*self.parsed_description.axis) + + @property + def lower_limit(self) -> Union[float, None]: + """ + :return: The lower limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.limit.lower + else: + return None + + @property + def upper_limit(self) -> Union[float, None]: + """ + :return: The upper limit of this joint, or None if the joint has no limits. + """ + if self.has_limits: + return self.parsed_description.limit.upper + else: + return None + + @property + def parent(self) -> str: + """ + :return: The name of the parent link of this joint. + """ + return self.parsed_description.parent + + @property + def child(self) -> str: + """ + :return: The name of the child link of this joint. + """ + return self.parsed_description.child + + @property + def damping(self) -> float: + """ + :return: The damping of this joint. + """ + return self.parsed_description.dynamics.damping + + @property + def friction(self) -> float: + """ + :return: The friction of this joint. + """ + return self.parsed_description.dynamics.friction + + +class ObjectDescription(AbstractObjectDescription): + """ + A class that represents an object description of an object. + """ + + class Link(AbstractObjectDescription.Link, LinkDescription): + ... + + class RootLink(AbstractObjectDescription.RootLink, Link): + ... + + class Joint(AbstractObjectDescription.Joint, JointDescription): + ... + + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child + joint name and the link name. + """ + return self.parsed_description.child_map + + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. + """ + return self.parsed_description.parent_map + + @property + def link_map(self) -> Dict[str, LinkDescription]: + """ + :return: A dictionary mapping the name of a link to its description. + """ + if self._link_map is None: + self._link_map = {link.name: link for link in self.links} + return self._link_map + + @property + def joint_map(self) -> Dict[str, JointDescription]: + """ + :return: A dictionary mapping the name of a joint to its description. + """ + if self._joint_map is None: + self._joint_map = {joint.name: joint for joint in self.joints} + return self._joint_map + + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: + if lower_limit is not None or upper_limit is not None: + limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) + else: + limit = None + if origin is not None: + origin = urdf.Pose(origin.position_as_list(), euler_from_quaternion(origin.orientation_as_list())) + if axis is not None: + axis = [axis.x, axis.y, axis.z] + if parent is None: + parent = self.get_root() + else: + parent = self.get_link_by_name(parent).parsed_description + joint = urdf.Joint(name, + parent, + self.get_link_by_name(child).parsed_description, + JointDescription.pycram_type_map[joint_type], + axis, origin, limit) + self.parsed_description.add_joint(joint) + if is_virtual: + self.virtual_joint_names.append(name) + + def load_description(self, path) -> URDF: + with open(path, 'r') as file: + # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them + with suppress_stdout_stderr(): + return URDF.from_xml_string(file.read()) + + def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: + """ + Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a + material tag in the URDF. + + :param path: The path to the mesh file. + :param name: The name of the object. + :param color: The color of the object. + :return: The absolute path of the created file + """ + urdf_template = ' \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + \n \ + ' + urdf_template = self.fix_missing_inertial(urdf_template) + rgb = " ".join(list(map(str, color.get_rgba()))) + pathlib_obj = pathlib.Path(path) + path = str(pathlib_obj.resolve()) + content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) + return content + + def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bool = True) -> str: + with open(path, mode="r") as f: + urdf_string = self.fix_missing_inertial(f.read()) + urdf_string = self.remove_error_tags(urdf_string) + urdf_string = self.fix_link_attributes(urdf_string) + try: + urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + return self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string + + def generate_from_parameter_server(self, name: str) -> str: + urdf_string = rospy.get_param(name) + urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + return self.fix_missing_inertial(urdf_string) + + @property + def joints(self) -> List[JointDescription]: + """ + :return: A list of joints descriptions of this object. + """ + if self._joints is None: + self._joints = [JointDescription(joint) for joint in self.parsed_description.joints] + return self._joints + + @property + def links(self) -> List[LinkDescription]: + """ + :return: A list of link descriptions of this object. + """ + if self._links is None: + self._links = [LinkDescription(link) for link in self.parsed_description.links] + return self._links + + def get_root(self) -> str: + """ + :return: the name of the root link of this object. + """ + return self.parsed_description.get_root() + + def get_tip(self) -> str: + """ + :return: the name of the tip link of this object. + :raises MultiplePossibleTipLinks: If there are multiple possible tip links. + """ + link = self.get_root() + while link in self.parsed_description.child_map: + children = self.parsed_description.child_map[link] + if len(children) > 1: + # Multiple children, can't decide which one to take (e.g. fingers of a hand) + raise MultiplePossibleTipLinks(self.parsed_description.name, link, [child[1] for child in children]) + else: + child = children[0][1] + link = child + return link + + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: + """ + :param start_link_name: The name of the start link of the chain. + :param end_link_name: The name of the end link of the chain. + :param joints: Whether to include joints in the chain. + :param links: Whether to include links in the chain. + :param fixed: Whether to include fixed joints in the chain. + :return: the chain of links from 'start_link_name' to 'end_link_name'. + """ + return self.parsed_description.get_chain(start_link_name, end_link_name, joints, links, fixed) + + @staticmethod + def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: + """ + Change paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) + can't deal with ROS package paths. + + :param urdf_string: The name of the URDf on the parameter server + :return: The URDF string with paths in the filesystem instead of ROS packages + """ + r = rospkg.RosPack() + new_urdf_string = "" + for line in urdf_string.split('\n'): + if "package://" in line: + s = line.split('//') + s1 = s[1].split('/') + path = r.get_path(s1[0]) + line = line.replace("package://" + s1[0], path) + new_urdf_string += line + '\n' + + return new_urdf_string + + @staticmethod + def make_mesh_paths_absolute(urdf_string: str, urdf_file_path: str) -> str: + """ + Convert all relative mesh paths in the URDF to absolute paths. + + :param urdf_string: The URDF description as string + :param urdf_file_path: The path to the URDF file + :returns: The new URDF description as string. + """ + # Parse the URDF file + root = ET.fromstring(urdf_string) + + # Iterate through all mesh tags + for mesh in root.findall('.//mesh'): + filename = mesh.attrib.get('filename', '') + if filename: + # If the filename is a relative path, convert it to an absolute path + if not os.path.isabs(filename): + # Deduce the base path from the relative path + base_path = os.path.dirname( + os.path.abspath(os.path.join(os.path.dirname(urdf_file_path), filename))) + abs_path = os.path.abspath(os.path.join(base_path, os.path.basename(filename))) + mesh.set('filename', abs_path) + + return ET.tostring(root, encoding='unicode') + + @staticmethod + def fix_missing_inertial(urdf_string: str) -> str: + """ + Insert inertial tags for every URDF link that has no inertia. + This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal + + :param urdf_string: The URDF description as string + :returns: The new, corrected URDF description as string. + """ + + inertia_tree = ET.ElementTree(ET.Element("inertial")) + inertia_tree.getroot().append(ET.Element("mass", {"value": "0.1"})) + inertia_tree.getroot().append(ET.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) + inertia_tree.getroot().append(ET.Element("inertia", {"ixx": "0.01", + "ixy": "0", + "ixz": "0", + "iyy": "0.01", + "iyz": "0", + "izz": "0.01"})) + + # create tree from string + tree = ET.ElementTree(ET.fromstring(urdf_string)) + + for link_element in tree.iter("link"): + inertial = [*link_element.iter("inertial")] + if len(inertial) == 0: + link_element.append(inertia_tree.getroot()) + + return ET.tostring(tree.getroot(), encoding='unicode') + + @staticmethod + def remove_error_tags(urdf_string: str) -> str: + """ + Remove all tags in the removing_tags list from the URDF since these tags are known to cause errors with the + URDF_parser + + :param urdf_string: String of the URDF from which the tags should be removed + :return: The URDF string with the tags removed + """ + tree = ET.ElementTree(ET.fromstring(urdf_string)) + removing_tags = ["gazebo", "transmission"] + for tag_name in removing_tags: + all_tags = tree.findall(tag_name) + for tag in all_tags: + tree.getroot().remove(tag) + + return ET.tostring(tree.getroot(), encoding='unicode') + + @staticmethod + def fix_link_attributes(urdf_string: str) -> str: + """ + Remove the attribute 'type' from links since this is not parsable by the URDF parser. + + :param urdf_string: The string of the URDF from which the attributes should be removed + :return: The URDF string with the attributes removed + """ + tree = ET.ElementTree(ET.fromstring(urdf_string)) + + for link in tree.iter("link"): + if "type" in link.attrib.keys(): + del link.attrib["type"] + + return ET.tostring(tree.getroot(), encoding='unicode') + + @staticmethod + def get_file_extension() -> str: + """ + :return: The file extension of the URDF file. + """ + return '.urdf' + + @property + def origin(self) -> Pose: + return Pose(self.parsed_description.origin.xyz, + quaternion_from_euler(*self.parsed_description.origin.rpy)) + + @property + def name(self) -> str: + return self.parsed_description.name + + +def parse_pose_from_body_element(body: mjcf.Element) -> Pose: + """ + Parse the pose from a body element. + + :param body: The body element. + :return: The pose of the body. + """ + position = body.pos + quaternion = body.quat + position = [0, 0, 0] if position is None else position + quaternion = [1, 0, 0, 0] if quaternion is None else quaternion + quaternion = [quaternion[1], quaternion[2], quaternion[3], quaternion[0]] + return Pose(position, quaternion) From 93aaac00bef879c69cc5b05fd5ced2c5d18eadf9 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 19 Aug 2024 22:04:21 +0200 Subject: [PATCH 147/189] [MultiverseMJCF] first implementation of mjcf and some tests for it. added dm_control in requirements.txt --- requirements.txt | 3 +- src/pycram/datastructures/enums.py | 1 + src/pycram/description.py | 16 ++ src/pycram/object_descriptors/mjcf.py | 348 ++++++++++---------------- src/pycram/object_descriptors/urdf.py | 2 +- test/test_mjcf.py | 36 +++ 6 files changed, 189 insertions(+), 217 deletions(-) create mode 100644 test/test_mjcf.py diff --git a/requirements.txt b/requirements.txt index ba876360f..78f065763 100644 --- a/requirements.txt +++ b/requirements.txt @@ -18,4 +18,5 @@ psutil==5.9.7 lxml==4.9.1 typing_extensions==4.9.0 owlready2>=0.45 -catkin_pkg \ No newline at end of file +catkin_pkg +dm_control \ No newline at end of file diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index ea7fdc464..3cb9b60e5 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -158,3 +158,4 @@ class MJCFJointType(Enum): BALL = "ball" SLIDE = "slide" HINGE = "hinge" + FIXED = "fixed" # Added for compatibility with PyCRAM, but not a real joint type in MuJoCo. diff --git a/src/pycram/description.py b/src/pycram/description.py index eae00eae2..5b260d022 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -718,6 +718,22 @@ def update_description_from_file(self, path: str) -> None: """ self._parsed_description = self.load_description(path) + def update_description_from_string(self, description_string: str) -> None: + """ + Update the description of this object from the given description string. + + :param description_string: The description string to update from. + """ + self._parsed_description = self.load_description_from_string(description_string) + + def load_description_from_string(self, description_string: str) -> Any: + """ + Load the description from the given string. + + :param description_string: The description string to load from. + """ + raise NotImplementedError + @property def parsed_description(self) -> Any: """ diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 8537183aa..073386af6 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -1,22 +1,17 @@ -import os import pathlib -import xml.etree.ElementTree as ET -import rospkg import rospy from geometry_msgs.msg import Point -from tf.transformations import quaternion_from_euler, euler_from_quaternion from typing_extensions import Union, List, Optional, Dict, Tuple from dm_control import mjcf -from ..datastructures.enums import JointType, MJCFGeomType +from ..datastructures.enums import JointType, MJCFGeomType, MJCFJointType from ..datastructures.pose import Pose from ..description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ SphereVisualShape, MeshVisualShape from ..failures import MultiplePossibleTipLinks -from ..utils import suppress_stdout_stderr class LinkDescription(AbstractLinkDescription): @@ -62,23 +57,21 @@ def name(self) -> str: class JointDescription(AbstractJointDescription): - urdf_type_map = {'unknown': JointType.UNKNOWN, - 'revolute': JointType.REVOLUTE, - 'continuous': JointType.CONTINUOUS, - 'prismatic': JointType.PRISMATIC, - 'floating': JointType.FLOATING, - 'planar': JointType.PLANAR, - 'fixed': JointType.FIXED} + mjcf_type_map = { + MJCFJointType.HINGE.value: JointType.REVOLUTE, + MJCFJointType.BALL.value: JointType.SPHERICAL, + MJCFJointType.SLIDE.value: JointType.PRISMATIC, + MJCFJointType.FREE.value: JointType.FLOATING + } - pycram_type_map = {pycram_type: urdf_type for urdf_type, pycram_type in urdf_type_map.items()} + pycram_type_map = {pycram_type: mjcf_type for mjcf_type, pycram_type in mjcf_type_map.items()} - def __init__(self, urdf_description: urdf.Joint, is_virtual: Optional[bool] = False): - super().__init__(urdf_description, is_virtual=is_virtual) + def __init__(self, mjcf_description: mjcf.Element, is_virtual: Optional[bool] = False): + super().__init__(mjcf_description, is_virtual=is_virtual) @property def origin(self) -> Pose: - return Pose(self.parsed_description.origin.xyz, - quaternion_from_euler(*self.parsed_description.origin.rpy)) + return parse_pose_from_body_element(self.parsed_description) @property def name(self) -> str: @@ -86,14 +79,14 @@ def name(self) -> str: @property def has_limits(self) -> bool: - return bool(self.parsed_description.limit) + return self.parsed_description.limited @property def type(self) -> JointType: """ :return: The type of this joint. """ - return self.urdf_type_map[self.parsed_description.type] + return self.mjcf_type_map[self.parsed_description.type] @property def axis(self) -> Point: @@ -108,7 +101,7 @@ def lower_limit(self) -> Union[float, None]: :return: The lower limit of this joint, or None if the joint has no limits. """ if self.has_limits: - return self.parsed_description.limit.lower + return self.parsed_description.range[0] else: return None @@ -118,7 +111,7 @@ def upper_limit(self) -> Union[float, None]: :return: The upper limit of this joint, or None if the joint has no limits. """ if self.has_limits: - return self.parsed_description.limit.upper + return self.parsed_description.range[1] else: return None @@ -127,28 +120,29 @@ def parent(self) -> str: """ :return: The name of the parent link of this joint. """ - return self.parsed_description.parent + return self._parent_link_element.parent.name @property def child(self) -> str: """ :return: The name of the child link of this joint. """ - return self.parsed_description.child + return self._parent_link_element.name + + @property + def _parent_link_element(self) -> mjcf.Element: + return self.parsed_description.parent @property def damping(self) -> float: """ :return: The damping of this joint. """ - return self.parsed_description.dynamics.damping + return self.parsed_description.damping @property def friction(self) -> float: - """ - :return: The friction of this joint. - """ - return self.parsed_description.dynamics.friction + raise NotImplementedError("Friction is not implemented for MJCF joints.") class ObjectDescription(AbstractObjectDescription): @@ -165,20 +159,57 @@ class RootLink(AbstractObjectDescription.RootLink, Link): class Joint(AbstractObjectDescription.Joint, JointDescription): ... + def __init__(self): + super().__init__() + self._link_map = None + self._joint_map = None + self._child_map = None + self._parent_map = None + self._links = None + self._joints = None + self.virtual_joint_names = [] + @property def child_map(self) -> Dict[str, List[Tuple[str, str]]]: """ :return: A dictionary mapping the name of a link to its children which are represented as a tuple of the child joint name and the link name. """ - return self.parsed_description.child_map + if self._child_map is None: + self._child_map = self._construct_child_map() + return self._child_map + + def _construct_child_map(self) -> Dict[str, List[Tuple[str, str]]]: + """ + Construct the child map of the object. + """ + child_map = {} + for joint in self.joints: + if joint.parent not in child_map: + child_map[joint.parent] = [(joint.name, joint.child)] + else: + child_map[joint.parent].append((joint.name, joint.child)) + return child_map @property def parent_map(self) -> Dict[str, Tuple[str, str]]: """ :return: A dictionary mapping the name of a link to its parent joint and link as a tuple. """ - return self.parsed_description.parent_map + if self._parent_map is None: + self._parent_map = self._construct_parent_map() + return self._parent_map + + def _construct_parent_map(self) -> Dict[str, Tuple[str, str]]: + """ + Construct the parent map of the object. + """ + child_map = self.child_map + parent_map = {} + for parent, children in child_map.items(): + for child in children: + parent_map[child[1]] = (child[0], parent) + return parent_map @property def link_map(self) -> Dict[str, LinkDescription]: @@ -202,85 +233,67 @@ def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, is_virtual: Optional[bool] = False) -> None: - if lower_limit is not None or upper_limit is not None: - limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) - else: - limit = None + + position: Optional[List[float]] = None + quaternion: Optional[List[float]] = None + lower_limit: float = 0.0 if lower_limit is None else lower_limit + upper_limit: float = 0.0 if upper_limit is None else upper_limit + limit = [lower_limit, upper_limit] + if origin is not None: - origin = urdf.Pose(origin.position_as_list(), euler_from_quaternion(origin.orientation_as_list())) + position = origin.position_as_list() + quaternion = origin.quaternion_as_list() + quaternion = [quaternion[1], quaternion[2], quaternion[3], quaternion[0]] if axis is not None: axis = [axis.x, axis.y, axis.z] - if parent is None: - parent = self.get_root() - else: - parent = self.get_link_by_name(parent).parsed_description - joint = urdf.Joint(name, - parent, - self.get_link_by_name(child).parsed_description, - JointDescription.pycram_type_map[joint_type], - axis, origin, limit) - self.parsed_description.add_joint(joint) + self.parsed_description.find(child).add('joint', name=name, type=JointDescription.pycram_type_map[joint_type], + axis=axis, pos=position, quat=quaternion, range=limit) if is_virtual: self.virtual_joint_names.append(name) - def load_description(self, path) -> URDF: - with open(path, 'r') as file: - # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them - with suppress_stdout_stderr(): - return URDF.from_xml_string(file.read()) + def load_description(self, path) -> mjcf.RootElement: + return mjcf.from_file(path) + + def load_description_from_string(self, description_string: str) -> mjcf.RootElement: + return mjcf.from_xml_string(description_string) def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: """ - Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a - material tag in the URDF. + Generate a mjcf xml file with the given .obj or .stl file as mesh. In addition, use the given rgba_color + to create a material tag in the xml. :param path: The path to the mesh file. :param name: The name of the object. :param color: The color of the object. - :return: The absolute path of the created file - """ - urdf_template = ' \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - \n \ - ' - urdf_template = self.fix_missing_inertial(urdf_template) - rgb = " ".join(list(map(str, color.get_rgba()))) - pathlib_obj = pathlib.Path(path) - path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - return content + :return: The generated xml string. + """ + # Create the MJCF model + model = mjcf.RootElement(model=f"{name}_object") + + # Add a body to the worldbody + main_body = model.worldbody.add('body', name=f"{name}_main") + + # add a free joint to the main body + joint = main_body.add('joint', name=f"{name}_main_joint", type=MJCFJointType.FREE.value) + + # Add the geometry (visual + collision combined) to the body + geom = main_body.add( + 'geom', + name=f"{name}_main_geom", + type='mesh', + mesh=str(pathlib.Path(path).resolve()), + rgba=" ".join(list(map(str, color.get_rgba()))), + scale=[1, 1, 1], + contype=1, + conaffinity=1 + ) + return model.to_xml_string() def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bool = True) -> str: - with open(path, mode="r") as f: - urdf_string = self.fix_missing_inertial(f.read()) - urdf_string = self.remove_error_tags(urdf_string) - urdf_string = self.fix_link_attributes(urdf_string) - try: - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) - urdf_string = self.fix_missing_inertial(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - return self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string + return mjcf.from_file(path) def generate_from_parameter_server(self, name: str) -> str: - urdf_string = rospy.get_param(name) - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) - return self.fix_missing_inertial(urdf_string) + return rospy.get_param(name) @property def joints(self) -> List[JointDescription]: @@ -288,7 +301,7 @@ def joints(self) -> List[JointDescription]: :return: A list of joints descriptions of this object. """ if self._joints is None: - self._joints = [JointDescription(joint) for joint in self.parsed_description.joints] + self._joints = [JointDescription(joint) for joint in self.parsed_description.find_all('joint')] return self._joints @property @@ -297,14 +310,19 @@ def links(self) -> List[LinkDescription]: :return: A list of link descriptions of this object. """ if self._links is None: - self._links = [LinkDescription(link) for link in self.parsed_description.links] + self._links = [LinkDescription(link) for link in self.parsed_description.find_all('body')] return self._links def get_root(self) -> str: """ :return: the name of the root link of this object. """ - return self.parsed_description.get_root() + if len(self.links) == 1: + return self.links[0].name + elif len(self.links) > 1: + return self.links[1].name + else: + raise ValueError("No links found in the object description.") def get_tip(self) -> str: """ @@ -312,11 +330,11 @@ def get_tip(self) -> str: :raises MultiplePossibleTipLinks: If there are multiple possible tip links. """ link = self.get_root() - while link in self.parsed_description.child_map: - children = self.parsed_description.child_map[link] + while link in self.child_map: + children = self.child_map[link] if len(children) > 1: # Multiple children, can't decide which one to take (e.g. fingers of a hand) - raise MultiplePossibleTipLinks(self.parsed_description.name, link, [child[1] for child in children]) + raise MultiplePossibleTipLinks(self.name, link, [child[1] for child in children]) else: child = children[0][1] link = child @@ -329,133 +347,33 @@ def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[b :param end_link_name: The name of the end link of the chain. :param joints: Whether to include joints in the chain. :param links: Whether to include links in the chain. - :param fixed: Whether to include fixed joints in the chain. + :param fixed: Whether to include fixed joints in the chain (Note: not used in MJCF). :return: the chain of links from 'start_link_name' to 'end_link_name'. """ - return self.parsed_description.get_chain(start_link_name, end_link_name, joints, links, fixed) - - @staticmethod - def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: - """ - Change paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) - can't deal with ROS package paths. - - :param urdf_string: The name of the URDf on the parameter server - :return: The URDF string with paths in the filesystem instead of ROS packages - """ - r = rospkg.RosPack() - new_urdf_string = "" - for line in urdf_string.split('\n'): - if "package://" in line: - s = line.split('//') - s1 = s[1].split('/') - path = r.get_path(s1[0]) - line = line.replace("package://" + s1[0], path) - new_urdf_string += line + '\n' - - return new_urdf_string - - @staticmethod - def make_mesh_paths_absolute(urdf_string: str, urdf_file_path: str) -> str: - """ - Convert all relative mesh paths in the URDF to absolute paths. - - :param urdf_string: The URDF description as string - :param urdf_file_path: The path to the URDF file - :returns: The new URDF description as string. - """ - # Parse the URDF file - root = ET.fromstring(urdf_string) - - # Iterate through all mesh tags - for mesh in root.findall('.//mesh'): - filename = mesh.attrib.get('filename', '') - if filename: - # If the filename is a relative path, convert it to an absolute path - if not os.path.isabs(filename): - # Deduce the base path from the relative path - base_path = os.path.dirname( - os.path.abspath(os.path.join(os.path.dirname(urdf_file_path), filename))) - abs_path = os.path.abspath(os.path.join(base_path, os.path.basename(filename))) - mesh.set('filename', abs_path) - - return ET.tostring(root, encoding='unicode') - - @staticmethod - def fix_missing_inertial(urdf_string: str) -> str: - """ - Insert inertial tags for every URDF link that has no inertia. - This is used to prevent Legacy(PyBullet) from dumping warnings in the terminal - - :param urdf_string: The URDF description as string - :returns: The new, corrected URDF description as string. - """ - - inertia_tree = ET.ElementTree(ET.Element("inertial")) - inertia_tree.getroot().append(ET.Element("mass", {"value": "0.1"})) - inertia_tree.getroot().append(ET.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"})) - inertia_tree.getroot().append(ET.Element("inertia", {"ixx": "0.01", - "ixy": "0", - "ixz": "0", - "iyy": "0.01", - "iyz": "0", - "izz": "0.01"})) - - # create tree from string - tree = ET.ElementTree(ET.fromstring(urdf_string)) - - for link_element in tree.iter("link"): - inertial = [*link_element.iter("inertial")] - if len(inertial) == 0: - link_element.append(inertia_tree.getroot()) - - return ET.tostring(tree.getroot(), encoding='unicode') - - @staticmethod - def remove_error_tags(urdf_string: str) -> str: - """ - Remove all tags in the removing_tags list from the URDF since these tags are known to cause errors with the - URDF_parser - - :param urdf_string: String of the URDF from which the tags should be removed - :return: The URDF string with the tags removed - """ - tree = ET.ElementTree(ET.fromstring(urdf_string)) - removing_tags = ["gazebo", "transmission"] - for tag_name in removing_tags: - all_tags = tree.findall(tag_name) - for tag in all_tags: - tree.getroot().remove(tag) - - return ET.tostring(tree.getroot(), encoding='unicode') - - @staticmethod - def fix_link_attributes(urdf_string: str) -> str: - """ - Remove the attribute 'type' from links since this is not parsable by the URDF parser. - - :param urdf_string: The string of the URDF from which the attributes should be removed - :return: The URDF string with the attributes removed - """ - tree = ET.ElementTree(ET.fromstring(urdf_string)) - - for link in tree.iter("link"): - if "type" in link.attrib.keys(): - del link.attrib["type"] - - return ET.tostring(tree.getroot(), encoding='unicode') + chain = [] + if links: + chain.append(end_link_name) + link = end_link_name + while link != start_link_name: + (joint, parent) = self.parent_map[link] + if joints: + chain.append(joint) + if links: + chain.append(parent) + link = parent + chain.reverse() + return chain @staticmethod def get_file_extension() -> str: """ :return: The file extension of the URDF file. """ - return '.urdf' + return '.xml' @property def origin(self) -> Pose: - return Pose(self.parsed_description.origin.xyz, - quaternion_from_euler(*self.parsed_description.origin.rpy)) + return parse_pose_from_body_element(self.parsed_description) @property def name(self) -> str: diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 6febc4468..218ee2f4a 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -248,7 +248,7 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = :param path: The path to the mesh file. :param name: The name of the object. :param color: The color of the object. - :return: The absolute path of the created file + :return: The URDF string. """ urdf_template = ' \n \ \n \ diff --git a/test/test_mjcf.py b/test/test_mjcf.py new file mode 100644 index 000000000..8581866ce --- /dev/null +++ b/test/test_mjcf.py @@ -0,0 +1,36 @@ +from unittest import TestCase +from dm_control import mjcf +from pycram.object_descriptors.mjcf import ObjectDescription as MJCFObjDesc + + +class TestMjcf(TestCase): + model: MJCFObjDesc + + @classmethod + def setUpClass(cls): + # Example usage + model = mjcf.RootElement("test") + + model.default.dclass = 'default' + + # Define a simple model with bodies and joints + body1 = model.worldbody.add('body', name='body1') + body2 = body1.add('body', name='body2') + joint1 = body2.add('joint', name='joint1', type='hinge') + + body3 = body2.add('body', name='body3') + joint2 = body3.add('joint', name='joint2', type='slide') + + cls.model = MJCFObjDesc() + print(model.to_xml_string()) + cls.model.update_description_from_string(model.to_xml_string()) + + def test_child_map(self): + self.assertEqual(self.model.child_map, {'body1': [('joint1', 'body2')], 'body2': [('joint2', 'body3')]}) + + def test_parent_map(self): + self.assertEqual(self.model.parent_map, {'body2': ('joint1', 'body1'), 'body3': ('joint2', 'body2')}) + + def test_get_chain(self): + self.assertEqual(self.model.get_chain('body1', 'body3'), + ['body1', 'joint1', 'body2', 'joint2', 'body3']) From 00f15b2117202f4021e4c4e32ef9246650b6a010 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 20 Aug 2024 10:05:54 +0200 Subject: [PATCH 148/189] [MultiverseMJCF] Corrected attribute name error. --- src/pycram/object_descriptors/mjcf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 073386af6..d83a523e4 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -242,7 +242,7 @@ def add_joint(self, name: str, child: str, joint_type: JointType, if origin is not None: position = origin.position_as_list() - quaternion = origin.quaternion_as_list() + quaternion = origin.orientation_as_list() quaternion = [quaternion[1], quaternion[2], quaternion[3], quaternion[0]] if axis is not None: axis = [axis.x, axis.y, axis.z] From 22cadb2a1f0830e972bc394b56d2228e82f91d89 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 20 Aug 2024 14:14:37 +0200 Subject: [PATCH 149/189] [MultiverseMJCF] made the cache manager manage the data directories as well. Allowed the change of the cache directory, because it is needed for multiverse. --- src/pycram/cache_manager.py | 11 +--- src/pycram/config/multiverse_conf.py | 26 +++++++-- src/pycram/config/world_conf.py | 7 ++- src/pycram/datastructures/world.py | 56 +++++++++++-------- src/pycram/worlds/multiverse.py | 22 +++----- .../worlds/multiverse_extras/helpers.py | 22 -------- test/test_bullet_world.py | 2 +- test/test_cache_manager.py | 5 +- test/test_multiverse.py | 6 +- 9 files changed, 80 insertions(+), 77 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index d99f8094b..98d53a49d 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -15,11 +15,6 @@ class CacheManager: The CacheManager is responsible for caching object description files and managing the cache directory. """ - mesh_extensions: List[str] = [".obj", ".stl"] - """ - The file extensions of mesh files. - """ - def __init__(self, cache_dir: str, data_directory: List[str]): """ Initialize the CacheManager. @@ -28,7 +23,7 @@ def __init__(self, cache_dir: str, data_directory: List[str]): :param data_directory: The directory where all resource files are stored. """ self.cache_dir = cache_dir - self.data_directory = data_directory + self.data_directories = data_directory self.clear_cache() def clear_cache(self): @@ -104,7 +99,7 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: :param path_object: The pathlib object of the file to look for. """ name = path_object.name - for data_dir in self.data_directory: + for data_dir in self.data_directories: data_path = pathlib.Path(data_dir).joinpath("**") for file in glob.glob(str(data_path), recursive=True): file_path = pathlib.Path(file) @@ -113,7 +108,7 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: return str(file_path) raise FileNotFoundError( - f"File {name} could not be found in the resource directory {self.data_directory}") + f"File {name} could not be found in the resource directory {self.data_directories}") def create_cache_dir_if_not_exists(self): """ diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 9b6c38750..5bf7d0f47 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -1,6 +1,15 @@ import datetime +import os + +from . import world_conf as world_conf +from ..worlds.multiverse_extras.helpers import find_multiverse_resources_path + +# Multiverse Configuration +resources_path: str = find_multiverse_resources_path() +""" +The path to the Multiverse resources directory. +""" -from . import world_conf as conf # Multiverse Socket Configuration HOST: str = "tcp://127.0.0.1" @@ -18,6 +27,13 @@ simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) +simulation_wait_time_factor: float = 1.0 +""" +The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for +the time taken by the simulation to process the request, this depends on the computational power of the machine +running the simulation. +""" + use_bullet_mode: bool = True """ If True, the simulation will always be in paused state unless the simulate() function is called, this behaves @@ -29,8 +45,8 @@ Only used when use_bullet_mode is False. This turns on the controller for the robot joints. """ -job_handling: conf.JobHandling = conf.JobHandling(let_pycram_move_attached_objects=False, - let_pycram_handle_spawning=False) +job_handling: world_conf.JobHandling = world_conf.JobHandling(let_pycram_move_attached_objects=False, + let_pycram_handle_spawning=False) -error_tolerance: conf.ErrorTolerance = conf.ErrorTolerance(acceptable_position_error=2e-2, - acceptable_prismatic_joint_position_error=2e-2) +error_tolerance: world_conf.ErrorTolerance = world_conf.ErrorTolerance(acceptable_position_error=2e-2, + acceptable_prismatic_joint_position_error=2e-2) diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index 8570b20c3..813860923 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -10,7 +10,12 @@ the objects. """ -cache_dir: str = os.path.join(resources_path, 'cached') +cache_dir_name: str = 'cached' +""" +The name of the cache directory. +""" + +cache_dir: str = os.path.join(resources_path, cache_dir_name) """ Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 5aaf59d71..84924c4cc 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -1,6 +1,7 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations +import os import threading import time from abc import ABC, abstractmethod @@ -9,7 +10,6 @@ import numpy as np import rospy from geometry_msgs.msg import Point -from tf.transformations import euler_from_quaternion, quaternion_multiply, quaternion_from_euler from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union from ..cache_manager import CacheManager @@ -65,24 +65,7 @@ class World(StateEntity, ABC): the URDF with the name of the URDF on the parameter server. """ - resources_path = conf.resources_path - """ - Global reference for the resources path, this is used to search for the description files of the robot and - the objects. - """ - - data_directory: List[str] = [resources_path] - """ - Global reference for the data directories, this is used to search for the description files of the robot, - the objects, and the cached files. - """ - - cache_dir: str = conf.cache_dir - """ - Global reference for the cache directory, this is used to cache the description files of the robot and the objects. - """ - - cache_manager: CacheManager = CacheManager(cache_dir, data_directory) + cache_manager: CacheManager = CacheManager(conf.cache_dir, [conf.resources_path]) """ Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ @@ -308,7 +291,7 @@ def simulation_time_step(self): """ The time step of the simulation in seconds. """ - return 1 / World.simulation_frequency + return 1 / self.__class__.simulation_frequency @abstractmethod def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, @@ -1022,15 +1005,44 @@ def register_two_objects_collision_callbacks(self, self.coll_callbacks[(object_a, object_b)] = CollisionCallbacks(on_collision_callback, on_collision_removal_callback) + @property + def data_directories(self): + """ + The resources directories where the objects, robots, and environments are stored. + """ + return self.cache_manager.data_directories + @classmethod - def add_resource_path(cls, path: str) -> None: + def add_resource_path(cls, path: str, prepend: bool = False) -> None: """ Adds a resource path in which the World will search for files. This resource directory is searched if an Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. + :param prepend: Put the new path at the beginning of the list such that it is searched first. + """ + if prepend: + cls.cache_manager.data_directories = [path] + cls.cache_manager.data_directories + else: + cls.cache_manager.data_directories.append(path) + + @classmethod + def remove_resource_path(cls, path: str) -> None: + """ + Remove the given path from the data_directories list. + + :param path: The path to remove. + """ + cls.cache_manager.data_directories.remove(path) + + @classmethod + def change_cache_dir_path(cls, path: str) -> None: + """ + Change the cache directory to the given path + + :param path: The new path for the cache directory. """ - cls.data_directory.append(path) + cls.cache_manager.cache_dir = os.path.join(path, conf.cache_dir_name) def get_prospection_object_for_object(self, obj: Object) -> Object: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 30f7e1772..656ce5221 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -5,14 +5,15 @@ import numpy as np import rospy from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple, Callable +from typing_extensions import List, Dict, Optional, Union, Tuple from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from .multiverse_datastructures.enums import MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from .multiverse_extras.helpers import find_multiverse_resources_path -from ..config import multiverse_conf as conf +from ..cache_manager import CacheManager +from ..config import multiverse_conf as conf, world_conf from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose @@ -58,10 +59,7 @@ class Multiverse(World): Whether to use the controller for the robot joints or not. """ - try: - simulation_wait_time_factor = float(os.environ['Multiverse_Simulation_Wait_Time_Factor']) - except KeyError: - simulation_wait_time_factor = 1.0 + simulation_wait_time_factor: float = conf.simulation_wait_time_factor """ The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for the time taken by the simulation to process the request, this depends on the computational power of the machine @@ -137,13 +135,12 @@ def _init_world(self, mode: WorldMode): def _make_sure_multiverse_resources_are_added(self): """ - Add the multiverse resources to the pycram world resources. + Add the multiverse resources to the pycram world resources, and change the data directory and cache manager. """ if not self.added_multiverse_resources: World.cache_manager.clear_cache() - dirname = find_multiverse_resources_path() - World.data_directory = [dirname] + self.data_directory - World.cache_manager.data_directory = World.data_directory + World.add_resource_path(conf.resources_path, prepend=True) + World.change_cache_dir_path(conf.resources_path) self.added_multiverse_resources = True def remove_multiverse_resources(self): @@ -151,9 +148,8 @@ def remove_multiverse_resources(self): Remove the multiverse resources from the pycram world resources. """ if self.added_multiverse_resources: - dirname = find_multiverse_resources_path() - World.data_directory.remove(dirname) - World.cache_manager.data_directory = World.data_directory + World.remove_resource_path(conf.resources_path) + World.change_cache_dir_path(world_conf.cache_dir) self.added_multiverse_resources = False def _spawn_floor(self): diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py index 049496c6d..72a31a86f 100644 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -39,28 +39,6 @@ def get_robot_mjcf_path(company_name: str, robot_name: str, xml_name: Optional[s return None -def get_resource_paths(dirname: str) -> List[str]: - resources_paths = ["../robots", "../worlds", "../objects"] - resources_paths = [ - os.path.join(dirname, resources_path.replace('../', '')) if not os.path.isabs( - resources_path) else resources_path - for resources_path in resources_paths - ] - - def add_directories(path: str) -> None: - with os.scandir(path) as entries: - for entry in entries: - if entry.is_dir(): - resources_paths.append(entry.path) - add_directories(entry.path) - - resources_path_copy = resources_paths.copy() - for resources_path in resources_path_copy: - add_directories(resources_path) - - return resources_paths - - def find_multiverse_resources_path() -> Optional[str]: """ Find the path to the Multiverse resources directory. diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index cd6990b5a..55ce2bb51 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -141,7 +141,7 @@ def test_equal_world_states(self): def test_add_resource_path(self): self.world.add_resource_path("test") - self.assertTrue("test" in self.world.data_directory) + self.assertTrue("test" in self.world.data_directories) def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 69006d458..848907973 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -3,15 +3,16 @@ from bullet_world_testcase import BulletWorldTestCase from pycram.object_descriptors.urdf import ObjectDescription as URDFObject +from pycram.config import world_conf as conf class TestCacheManager(BulletWorldTestCase): def test_generate_description_and_write_to_cache(self): cache_manager = self.world.cache_manager - path = os.path.join(self.world.resources_path, "objects/apartment.urdf") + path = os.path.join(conf.resources_path, "objects/apartment.urdf") extension = Path(path).suffix - cache_path = os.path.join(self.world.cache_dir, "apartment.urdf") + cache_path = os.path.join(conf.cache_dir, "apartment.urdf") apartment = URDFObject(path) cache_manager.generate_description_and_write_to_cache(path, "apartment", extension, cache_path, apartment) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4849e0b5f..f0364e344 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -286,7 +286,7 @@ def test_attach_with_robot(self): def test_get_object_contact_points(self): for i in range(3): - milk = self.spawn_milk([1, 1, 0], [0, -0.707, 0, 0.707]) + milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -295,7 +295,7 @@ def test_get_object_contact_points(self): cup = self.spawn_cup([1, 1, 0.1]) # 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) + self.multiverse.simulate(0.2) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) @@ -309,7 +309,7 @@ def test_get_contact_points_between_two_objects(self): cup = self.spawn_cup([1, 1, 0.1]) # 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) + self.multiverse.simulate(0.2) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From 50cd3125b3e5899189830335f98bfec74cc104c8 Mon Sep 17 00:00:00 2001 From: Nils Leusmann Date: Tue, 20 Aug 2024 17:01:52 +0200 Subject: [PATCH 150/189] Adds file// to the replaced key words --- src/pycram/object_descriptors/urdf.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index e8a907bab..270646dea 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -244,7 +244,7 @@ def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bo urdf_string = self.remove_error_tags(urdf_string) urdf_string = self.fix_link_attributes(urdf_string) try: - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) urdf_string = self.fix_missing_inertial(urdf_string) except rospkg.ResourceNotFound as e: rospy.logerr(f"Could not find resource package linked in this URDF") @@ -253,7 +253,7 @@ def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bo def generate_from_parameter_server(self, name: str) -> str: urdf_string = rospy.get_param(name) - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) return self.fix_missing_inertial(urdf_string) def get_link_by_name(self, link_name: str) -> LinkDescription: @@ -313,10 +313,10 @@ def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: return self.parsed_description.get_chain(start_link_name, end_link_name) @staticmethod - def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: + def replace_relative_references_with_absolute_paths(urdf_string: str) -> str: """ - Change paths for files in the URDF from ROS paths to paths in the file system. Since World (PyBullet legacy) - can't deal with ROS package paths. + Change paths for files in the URDF from ROS paths and file dir references to paths in the file system. Since + World (PyBullet legacy) can't deal with ROS package paths. :param urdf_string: The name of the URDf on the parameter server :return: The URDF string with paths in the filesystem instead of ROS packages @@ -329,6 +329,8 @@ def replace_ros_package_references_to_absolute_paths(urdf_string: str) -> str: s1 = s[1].split('/') path = r.get_path(s1[0]) line = line.replace("package://" + s1[0], path) + if 'file://' in line: + line = line.replace("file://", './') new_urdf_string += line + '\n' return new_urdf_string From 08d9cd31f77395b7fb04f564d6253fe9effd0060 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 20 Aug 2024 18:00:00 +0200 Subject: [PATCH 151/189] [MultiverseMJCF] Implemented and tests creating mujoco xml files from mesh files. --- src/pycram/cache_manager.py | 29 +-------- src/pycram/description.py | 62 +++++++++++++------ src/pycram/object_descriptors/mjcf.py | 85 +++++++++++++++++---------- src/pycram/object_descriptors/urdf.py | 36 ++++++------ test/test_cache_manager.py | 3 +- test/test_description.py | 15 +++-- test/test_multiverse.py | 5 ++ 7 files changed, 136 insertions(+), 99 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 98d53a49d..0be43296f 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -62,35 +62,10 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, if not self.is_cached(path, object_description) or ignore_cached_files: # if file is not yet cached preprocess the description file and save it in the cache directory. path = self.look_for_file_in_data_dir(path_object) - self.generate_description_and_write_to_cache(path, object_name, extension, cache_path, object_description) + object_description.generate_description_from_file(path, object_name, extension, cache_path) return cache_path - def generate_description_and_write_to_cache(self, path: str, name: str, extension: str, cache_path: str, - object_description: 'ObjectDescription') -> None: - """ - Generate the description from the file at the given path and write it to the cache directory. - - :param path: The path of the file to preprocess. - :param name: The name of the object. - :param extension: The file extension of the file to preprocess. - :param cache_path: The path of the file in the cache directory. - :param object_description: The object description of the file. - """ - description_string = object_description.generate_description_from_file(path, name, extension) - self.write_to_cache(description_string, cache_path) - - @staticmethod - def write_to_cache(description_string: str, cache_path: str) -> None: - """ - Write the description string to the cache directory. - - :param description_string: The description string to write to the cache directory. - :param cache_path: The path of the file in the cache directory. - """ - with open(cache_path, "w") as file: - file.write(description_string) - def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: """ Look for a file in the data directory of the World. If the file is not found in the data directory, raise a @@ -148,5 +123,5 @@ def check_without_extension(self, path: str, object_description: 'ObjectDescript :param object_description: The object description of the file. """ file_stem = pathlib.Path(path).stem - full_path = pathlib.Path(self.cache_dir + file_stem + object_description.get_file_extension()) + full_path = pathlib.Path(os.path.join(self.cache_dir, file_stem + object_description.get_file_extension())) return full_path.exists() diff --git a/src/pycram/description.py b/src/pycram/description.py index 5b260d022..82143a994 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -1,6 +1,7 @@ from __future__ import annotations import logging +import os import pathlib from abc import ABC, abstractmethod @@ -758,33 +759,56 @@ def load_description(self, path: str) -> Any: """ pass - def generate_description_from_file(self, path: str, name: str, extension: str) -> str: + def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str) -> None: """ - Generate and preprocess the description from the file at the given path and return the preprocessed - description as a string. + Generate and preprocess the description from the file at the given path and save the preprocessed + description. The generated description will be saved at the given save path. :param path: The path of the file to preprocess. :param name: The name of the object. :param extension: The file extension of the file to preprocess. - :return: The processed description string. + :param save_path: The path to save the generated description file. + :raises ObjectDescriptionNotFound: If the description file could not be found/read. """ - description_string = None if extension in self.mesh_extensions: - description_string = self.generate_from_mesh_file(path, name) + self.generate_from_mesh_file(path, name, save_path=save_path) elif extension == self.get_file_extension(): - description_string = self.generate_from_description_file(path) + self.generate_from_description_file(path, save_path=save_path) else: try: # Using the description from the parameter server - description_string = self.generate_from_parameter_server(path) + self.generate_from_parameter_server(path, save_path=save_path) except KeyError: logging.warning(f"Couldn't find file data in the ROS parameter server") - if description_string is None: + if not self.check_description_file_exists_and_can_be_read(save_path): raise ObjectDescriptionNotFound(name, path, extension) - return description_string + @staticmethod + def check_description_file_exists_and_can_be_read(path: str) -> bool: + """ + Check if the description file exists at the given path. + + :param path: The path to the description file. + :return: True if the file exists, False otherwise. + """ + exists = os.path.exists(path) + if exists: + with open(path, "r") as file: + exists = bool(file.read()) + return exists + + @staticmethod + def write_description_to_file(description_string: str, save_path: str) -> None: + """ + Write the description string to the file at the given path. + + :param description_string: The description string to write. + :param save_path: The path of the file to write to. + """ + with open(save_path, "w") as file: + file.write(description_string) def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: """ @@ -806,37 +830,39 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @classmethod @abstractmethod - def generate_from_mesh_file(cls, path: str, name: str) -> str: + def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> None: """ Generate a description file from one of the mesh types defined in the mesh_extensions and - return the path of the generated file. + return the path of the generated file. The generated file will be saved at the given save_path. :param path: The path to the .obj file. :param name: The name of the object. - :return: The path of the generated description file. + :param save_path: The path to save the generated description file. """ pass @classmethod @abstractmethod - def generate_from_description_file(cls, path: str, make_mesh_paths_absolute: bool = True) -> str: + def generate_from_description_file(cls, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: """ - Preprocess the given file and return the preprocessed description string. + Preprocess the given file and return the preprocessed description string. The preprocessed description will be + saved at the given save_path. :param path: The path of the file to preprocess. + :param save_path: The path to save the preprocessed description file. :param make_mesh_paths_absolute: Whether to make the mesh paths absolute. - :return: The preprocessed description string. """ pass @classmethod @abstractmethod - def generate_from_parameter_server(cls, name: str) -> str: + def generate_from_parameter_server(cls, name: str, save_path: str) -> None: """ Preprocess the description from the ROS parameter server and return the preprocessed description string. + The preprocessed description will be saved at the given save_path. :param name: The name of the description on the parameter server. - :return: The preprocessed description string. + :param save_path: The path to save the preprocessed description file. """ pass diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index d83a523e4..c102dea4a 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -1,6 +1,8 @@ import pathlib +import numpy as np import rospy +import os from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, Dict, Tuple from dm_control import mjcf @@ -13,6 +15,13 @@ SphereVisualShape, MeshVisualShape from ..failures import MultiplePossibleTipLinks +from multiverse_parser import Configuration, Factory +from multiverse_parser import (WorldBuilder, + BodyBuilder, + GeomType, GeomProperty, + MeshProperty) +from multiverse_parser import MjcfExporter, UrdfExporter +from pxr import Usd, UsdGeom class LinkDescription(AbstractLinkDescription): """ @@ -144,6 +153,33 @@ def damping(self) -> float: def friction(self) -> float: raise NotImplementedError("Friction is not implemented for MJCF joints.") +class ObjectFactory(Factory): + def __init__(self, object_name: str, file_path: str, config: Configuration): + super().__init__(file_path, config) + + self._world_builder = WorldBuilder(usd_file_path=self.tmp_usd_file_path) + + body_builder = self._world_builder.add_body(body_name=object_name) + + tmp_usd_mesh_file_path, tmp_origin_mesh_file_path = self.import_mesh( + mesh_file_path=file_path, merge_mesh=True) + mesh_stage = Usd.Stage.Open(tmp_usd_mesh_file_path) + for idx, mesh_prim in enumerate([prim for prim in mesh_stage.Traverse() if prim.IsA(UsdGeom.Mesh)]): + mesh_name = mesh_prim.GetName() + mesh_path = mesh_prim.GetPath() + mesh_property = MeshProperty.from_mesh_file_path(mesh_file_path=tmp_usd_mesh_file_path, + mesh_path=mesh_path) + geom_property = GeomProperty(geom_type=GeomType.MESH, + is_visible=False, + is_collidable=True) + geom_builder = body_builder.add_geom(geom_name=f"SM_{object_name}_mesh_{idx}", + geom_property=geom_property) + geom_builder.add_mesh(mesh_name=mesh_name, mesh_property=mesh_property) + + def export_to_mjcf(self, output_file_path: str): + exporter = MjcfExporter(self, output_file_path) + exporter.build() + exporter.export(keep_usd=False) class ObjectDescription(AbstractObjectDescription): """ @@ -252,12 +288,13 @@ def add_joint(self, name: str, child: str, joint_type: JointType, self.virtual_joint_names.append(name) def load_description(self, path) -> mjcf.RootElement: - return mjcf.from_file(path) + return mjcf.from_file(path, model_dir=pathlib.Path(path).parent) def load_description_from_string(self, description_string: str) -> mjcf.RootElement: return mjcf.from_xml_string(description_string) - def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: + def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color(), + save_path: Optional[str] = None) -> None: """ Generate a mjcf xml file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a material tag in the xml. @@ -265,35 +302,21 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = :param path: The path to the mesh file. :param name: The name of the object. :param color: The color of the object. - :return: The generated xml string. - """ - # Create the MJCF model - model = mjcf.RootElement(model=f"{name}_object") - - # Add a body to the worldbody - main_body = model.worldbody.add('body', name=f"{name}_main") - - # add a free joint to the main body - joint = main_body.add('joint', name=f"{name}_main_joint", type=MJCFJointType.FREE.value) - - # Add the geometry (visual + collision combined) to the body - geom = main_body.add( - 'geom', - name=f"{name}_main_geom", - type='mesh', - mesh=str(pathlib.Path(path).resolve()), - rgba=" ".join(list(map(str, color.get_rgba()))), - scale=[1, 1, 1], - contype=1, - conaffinity=1 - ) - return model.to_xml_string() - - def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bool = True) -> str: - return mjcf.from_file(path) - - def generate_from_parameter_server(self, name: str) -> str: - return rospy.get_param(name) + :param save_path: The path to save the generated xml file. + """ + factory = ObjectFactory(object_name=name, file_path=path, + config=Configuration(model_name=name, + fixed_base=False, + default_rgba=np.array(color.get_rgba()))) + factory.export_to_mjcf(output_file_path=save_path) + + def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: + mjcf_string = mjcf.from_file(path) + self.write_description_to_file(mjcf_string, save_path) + + def generate_from_parameter_server(self, name: str, save_path: str) -> None: + mjcf_string = rospy.get_param(name) + self.write_description_to_file(mjcf_string, save_path) @property def joints(self) -> List[JointDescription]: diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 218ee2f4a..bb17c282e 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -240,15 +240,15 @@ def load_description(self, path) -> URDF: with suppress_stdout_stderr(): return URDF.from_xml_string(file.read()) - def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = Color()) -> str: + def generate_from_mesh_file(self, path: str, name: str, save_path: str, color: Optional[Color] = Color()) -> None: """ Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a - material tag in the URDF. + material tag in the URDF. The URDF file will be saved to the given save_path. :param path: The path to the mesh file. :param name: The name of the object. + :param save_path: The path to save the URDF file to. :param color: The color of the object. - :return: The URDF string. """ urdf_template = ' \n \ \n \ @@ -273,25 +273,27 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) - return content + self.write_description_to_file(content, save_path) - def generate_from_description_file(self, path: str, make_mesh_paths_absolute: bool = True) -> str: + def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: with open(path, mode="r") as f: urdf_string = self.fix_missing_inertial(f.read()) - urdf_string = self.remove_error_tags(urdf_string) - urdf_string = self.fix_link_attributes(urdf_string) - try: - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) - urdf_string = self.fix_missing_inertial(urdf_string) - except rospkg.ResourceNotFound as e: - rospy.logerr(f"Could not find resource package linked in this URDF") - raise e - return self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string - - def generate_from_parameter_server(self, name: str) -> str: + urdf_string = self.remove_error_tags(urdf_string) + urdf_string = self.fix_link_attributes(urdf_string) + try: + urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) + except rospkg.ResourceNotFound as e: + rospy.logerr(f"Could not find resource package linked in this URDF") + raise e + urdf_string = self.make_mesh_paths_absolute(urdf_string, path) if make_mesh_paths_absolute else urdf_string + self.write_description_to_file(urdf_string, save_path) + + def generate_from_parameter_server(self, name: str, save_path: str) -> None: urdf_string = rospy.get_param(name) urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) - return self.fix_missing_inertial(urdf_string) + urdf_string = self.fix_missing_inertial(urdf_string) + self.write_description_to_file(urdf_string, save_path) @property def joints(self) -> List[JointDescription]: diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 848907973..118650c3b 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -14,6 +14,5 @@ def test_generate_description_and_write_to_cache(self): extension = Path(path).suffix cache_path = os.path.join(conf.cache_dir, "apartment.urdf") apartment = URDFObject(path) - cache_manager.generate_description_and_write_to_cache(path, "apartment", extension, cache_path, - apartment) + apartment.generate_description_from_file(path, "apartment", extension, cache_path) self.assertTrue(cache_manager.is_cached(path, apartment)) diff --git a/test/test_description.py b/test/test_description.py index a0d12b3b2..e324384ac 100644 --- a/test/test_description.py +++ b/test/test_description.py @@ -1,3 +1,4 @@ +import os.path import pathlib from bullet_world_testcase import BulletWorldTestCase @@ -22,10 +23,16 @@ def test_joint_child_link(self): def test_generate_description_from_mesh(self): file_path = pathlib.Path(__file__).parent.resolve() - self.assertTrue(self.milk.description.generate_description_from_file(str(file_path) + "/../resources/cached/milk.stl", - "milk", ".stl")) + cache_path = self.world.cache_manager.cache_dir + cache_path = os.path.join(cache_path, f"{self.milk.description.name}.urdf") + self.milk.description.generate_from_mesh_file(str(file_path) + "/../resources/milk.stl", "milk", cache_path) + self.assertTrue(self.world.cache_manager.is_cached(f"{self.milk.name}", self.milk.description)) def test_generate_description_from_description_file(self): file_path = pathlib.Path(__file__).parent.resolve() - self.assertTrue(self.milk.description.generate_description_from_file(str(file_path) + "/../resources/cached/milk.urdf", - "milk", ".urdf")) + file_extension = self.robot.description.get_file_extension() + pr2_path = str(file_path) + f"/../resources/robots/{self.robot.description.name}{file_extension}" + cache_path = self.world.cache_manager.cache_dir + cache_path = os.path.join(cache_path, f"{self.robot.description.name}.urdf") + self.robot.description.generate_from_description_file(pr2_path, cache_path) + self.assertTrue(self.world.cache_manager.is_cached(self.robot.name, self.robot.description)) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index f0364e344..e04329c56 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -14,6 +14,7 @@ from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions from pycram.worlds.multiverse_extras.helpers import get_robot_mjcf_path, parse_mjcf_actuators +from pycram.object_descriptors.mjcf import ObjectDescription as MJCF multiverse_installed = True try: @@ -53,6 +54,10 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.reset_world_and_remove_objects() + def test_spawn_mesh_object(self): + milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0.1])) + assert milk is not None + def test_parse_mjcf_actuators(self): mjcf_file = get_robot_mjcf_path("pal_robotics", "tiago_dual") self.assertTrue(os.path.exists(mjcf_file)) From 9759ab8fdb40ecd0921091672b017868041c4bfd Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 20 Aug 2024 18:42:01 +0200 Subject: [PATCH 152/189] [MultiverseMJCF] currently generated xml descriptions from mesh files does not have free base and does not collide. --- test/test_multiverse.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index e04329c56..c8d295d9f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -55,8 +55,10 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() def test_spawn_mesh_object(self): - milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0.1])) - assert milk is not None + milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0])) + self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0])) + contact_points = milk.contact_points() + self.assertTrue(len(contact_points) > 0) def test_parse_mjcf_actuators(self): mjcf_file = get_robot_mjcf_path("pal_robotics", "tiago_dual") From 70a166bff56a70ae539679ee30bff2fc626fa059 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 12:41:30 +0200 Subject: [PATCH 153/189] [MultiverseMJCF] Now the generated xml file from mesh has auto computed inertia, a free joint, and can collide --- src/pycram/object_descriptors/mjcf.py | 16 +++++++++++----- test/test_multiverse.py | 5 +++-- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index c102dea4a..051da0618 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -15,14 +15,14 @@ SphereVisualShape, MeshVisualShape from ..failures import MultiplePossibleTipLinks -from multiverse_parser import Configuration, Factory +from multiverse_parser import Configuration, Factory, InertiaSource from multiverse_parser import (WorldBuilder, - BodyBuilder, GeomType, GeomProperty, MeshProperty) -from multiverse_parser import MjcfExporter, UrdfExporter +from multiverse_parser import MjcfExporter from pxr import Usd, UsdGeom + class LinkDescription(AbstractLinkDescription): """ A class that represents a link description of an object. @@ -95,7 +95,10 @@ def type(self) -> JointType: """ :return: The type of this joint. """ - return self.mjcf_type_map[self.parsed_description.type] + if hasattr(self.parsed_description, 'type'): + return self.mjcf_type_map[self.parsed_description.type] + else: + return self.mjcf_type_map[MJCFJointType.FREE.value] @property def axis(self) -> Point: @@ -175,11 +178,14 @@ def __init__(self, object_name: str, file_path: str, config: Configuration): geom_builder = body_builder.add_geom(geom_name=f"SM_{object_name}_mesh_{idx}", geom_property=geom_property) geom_builder.add_mesh(mesh_name=mesh_name, mesh_property=mesh_property) + geom_builder.build() + + body_builder.compute_and_set_inertial(inertia_source=InertiaSource.FROM_COLLISION_MESH) def export_to_mjcf(self, output_file_path: str): exporter = MjcfExporter(self, output_file_path) exporter.build() - exporter.export(keep_usd=False) + exporter.export(keep_usd=True) class ObjectDescription(AbstractObjectDescription): """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index c8d295d9f..01e9920ee 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -55,8 +55,9 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() def test_spawn_mesh_object(self): - milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0])) - self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0])) + milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0.1])) + self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) + self.multiverse.simulate(0.2) contact_points = milk.contact_points() self.assertTrue(len(contact_points) > 0) From 03a08afdeb6444cfba2e3a71613f8b8ca3df9003 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 14:50:10 +0200 Subject: [PATCH 154/189] [MultiverseMJCF] Added an object description resolver such that, when initializing world object there is no need for specifying the description if it can be inferred from the path. --- demos/pycram_multiverse_demo/demo.py | 3 +- src/pycram/config/multiverse_conf.py | 10 +++++- src/pycram/config/world_conf.py | 9 +++++- src/pycram/datastructures/dataclasses.py | 1 - src/pycram/datastructures/world.py | 7 +++- src/pycram/failures.py | 19 ++++++++++- src/pycram/object_descriptors/mjcf.py | 5 +-- src/pycram/world_concepts/world_object.py | 39 ++++++++++++++++++++--- src/pycram/worlds/bullet_world.py | 4 +-- src/pycram/worlds/multiverse.py | 12 ++++--- test/bullet_world_testcase.py | 4 +-- test/test_bullet_world.py | 3 +- test/test_failure_handling.py | 4 +-- test/test_multiverse.py | 6 ++-- test/test_object.py | 4 ++- 15 files changed, 98 insertions(+), 32 deletions(-) diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index cb5b36508..abf9cadda 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -19,7 +19,6 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform() - return object_desig @@ -28,7 +27,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): robot = Object('pr2', ObjectType.ROBOT, f'pr2{extension}', pose=Pose([1.3, 2, 0.01])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") -milk = Object("pycram_milk", ObjectType.MILK, f"pycram_milk{extension}", pose=Pose([2.4, 2, 1.02]), +milk = Object("milk", ObjectType.MILK, f"milk.stl", pose=Pose([2.4, 2, 1.02]), color=Color(1, 0, 0, 1)) spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.5, 2.2, 0.85]), diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 5bf7d0f47..56d9b6cf7 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -1,8 +1,11 @@ import datetime -import os + +from typing_extensions import Type from . import world_conf as world_conf +from ..description import ObjectDescription from ..worlds.multiverse_extras.helpers import find_multiverse_resources_path +from ..object_descriptors.mjcf import ObjectDescription as MJCF # Multiverse Configuration resources_path: str = find_multiverse_resources_path() @@ -45,6 +48,11 @@ Only used when use_bullet_mode is False. This turns on the controller for the robot joints. """ +default_description_type: Type[ObjectDescription] = MJCF +""" +The default description type for the objects. +""" + job_handling: world_conf.JobHandling = world_conf.JobHandling(let_pycram_move_attached_objects=False, let_pycram_handle_spawning=False) diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index 813860923..045b7c5f5 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -2,7 +2,9 @@ import os from dataclasses import dataclass -from typing_extensions import Tuple +from typing_extensions import Tuple, Type +from ..description import ObjectDescription +from ..object_descriptors.urdf import ObjectDescription as URDF resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') """ @@ -35,6 +37,11 @@ Whether to use in debug mode. (This is used to print debug messages, plot images, etc.) """ +default_description_type: Type[ObjectDescription] = URDF +""" +The default description type for the objects. +""" + @dataclass class JobHandling: diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index f7dcdab28..ef890d603 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -8,7 +8,6 @@ from .enums import JointType, Shape, VirtualMoveBaseJointName from .pose import Pose, Point from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable -from ..config import world_conf as conf if TYPE_CHECKING: from ..description import Link diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 84924c4cc..41f0aee7c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -10,7 +10,7 @@ import numpy as np import rospy from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union +from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type from ..cache_manager import CacheManager from ..config import world_conf as conf @@ -104,6 +104,11 @@ class World(StateEntity, ABC): Whether to raise an error if the goals are not achieved. """ + default_description_type: Type[ObjectDescription] = conf.default_description_type + """ + The default description type for objects (e.g. URDF, MJCF, etc.). + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, **config_kwargs): """ diff --git a/src/pycram/failures.py b/src/pycram/failures.py index c28e5ed3b..2fc2a59f8 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -1,3 +1,5 @@ +from pathlib import Path + from typing_extensions import TYPE_CHECKING, List if TYPE_CHECKING: @@ -133,8 +135,10 @@ def __init__(self, *args, **kwargs): class IKError(PlanFailure): """Thrown when no inverse kinematics solution could be found""" + def __init__(self, pose, base_frame, tip_frame): - self.message = "Position {} in frame '{}' is not reachable for end effector: '{}'".format(pose, base_frame, tip_frame) + self.message = "Position {} in frame '{}' is not reachable for end effector: '{}'".format(pose, base_frame, + tip_frame) super(IKError, self).__init__(self.message) @@ -445,3 +449,16 @@ class MultiplePossibleTipLinks(Exception): def __init__(self, object_name: str, start_link: str, tip_links: List[str]): super().__init__(f"Multiple possible tip links found for object {object_name} with start link {start_link}:" f" {tip_links}") + + +class UnsupportedFileExtension(Exception): + def __init__(self, object_name: str, path: str): + extension = Path(path).suffix + super().__init__(f"Unsupported file extension for object {object_name} with path {path}" + f"and extension {extension}") + + +class ObjectDescriptionUndefined(Exception): + def __init__(self, object_name: str): + super().__init__(f"Object description for object {object_name} is not defined, eith a path or a description" + f"object should be provided.") diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 051da0618..22c19d343 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -2,7 +2,6 @@ import numpy as np import rospy -import os from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, Dict, Tuple from dm_control import mjcf @@ -156,6 +155,7 @@ def damping(self) -> float: def friction(self) -> float: raise NotImplementedError("Friction is not implemented for MJCF joints.") + class ObjectFactory(Factory): def __init__(self, object_name: str, file_path: str, config: Configuration): super().__init__(file_path, config) @@ -185,7 +185,8 @@ def __init__(self, object_name: str, file_path: str, config: Configuration): def export_to_mjcf(self, output_file_path: str): exporter = MjcfExporter(self, output_file_path) exporter.build() - exporter.export(keep_usd=True) + exporter.export(keep_usd=False) + class ObjectDescription(AbstractObjectDescription): """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index c1a6da4f9..c117472ca 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -2,6 +2,7 @@ import logging import os +from pathlib import Path import numpy as np import rospy @@ -16,9 +17,11 @@ from ..datastructures.world import World from ..datastructures.world_entity import WorldEntity from ..description import ObjectDescription, LinkDescription, Joint -from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects +from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ + ObjectDescriptionUndefined from ..local_transformer import LocalTransformer -from ..object_descriptors.urdf import ObjectDescription as URDFObject +from ..object_descriptors.urdf import ObjectDescription as URDF +from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment @@ -35,8 +38,14 @@ class Object(WorldEntity): The prefix for the tf frame of objects in the prospection world. """ + extension_to_description_type: Dict[str, Type[ObjectDescription]] = {URDF.get_file_extension(): URDF, + MJCF.get_file_extension(): MJCF} + """ + A dictionary that maps the file extension to the corresponding ObjectDescription type. + """ + def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Type[ObjectDescription] = URDFObject, + description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, color: Color = Color(), @@ -66,7 +75,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.name: str = name self.obj_type: ObjectType = obj_type self.color: Color = color - self.description = description() + self._resolve_description(path, description) self.cache_manager = self.world.cache_manager self.local_transformer = LocalTransformer() @@ -95,6 +104,26 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.world.objects.append(self) + def _resolve_description(self, path: Optional[str] = None, description: Optional[ObjectDescription] = None) -> None: + """ + Find the correct description type of the object and initialize it and set the description of this object to it. + + :param path: The path to the source file. + :param description: The ObjectDescription of the object. + """ + if description is not None: + self.description = description + return + if path is None: + raise ObjectDescriptionUndefined(self.name) + extension = Path(path).suffix + if extension in self.extension_to_description_type: + self.description = self.extension_to_description_type[extension]() + elif extension in ObjectDescription.mesh_extensions: + self.description = self.world.default_description_type() + else: + raise UnsupportedFileExtension(self.name, path) + def set_mobile_robot_pose(self, pose: Pose) -> None: """ Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when @@ -1342,7 +1371,7 @@ def copy_to_world(self, world: World) -> Object: :param world: The world to which the object should be copied. :return: The copied object in the given world. """ - obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), + obj = Object(self.name, self.obj_type, self.path, self.description, self.get_pose(), world, self.color) return obj diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index ca4de1fe4..3725e01c1 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -34,8 +34,6 @@ class is the main interface to the Bullet Physics Engine and should be used to s manipulate the Bullet World. """ - extension: str = ObjectDescription.get_file_extension() - # Check is for sphinx autoAPI to be able to work in a CI workflow if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') @@ -63,7 +61,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - _ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension, + _ = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", world=self) def _init_world(self, mode: WorldMode): diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 656ce5221..e73727aac 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -1,24 +1,21 @@ import logging -import os from time import sleep import numpy as np import rospy from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple +from typing_extensions import List, Dict, Optional, Union, Tuple, Type from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from .multiverse_datastructures.enums import MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD -from .multiverse_extras.helpers import find_multiverse_resources_path -from ..cache_manager import CacheManager from ..config import multiverse_conf as conf, world_conf from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType from ..datastructures.pose import Pose from ..datastructures.world import World -from ..description import Link, Joint +from ..description import Link, Joint, ObjectDescription from ..robot_description import RobotDescription from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses @@ -66,6 +63,11 @@ class Multiverse(World): running the simulation. """ + default_description_type: Type[ObjectDescription] = conf.default_description_type + """ + The default description type for the objects. + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 7e572500b..dc6abc8c6 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -30,7 +30,7 @@ def setUpClass(cls): RobotDescription.current_robot_description.name + cls.extension) cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False cls.viz_marker_publisher = VizMarkerPublisher() OntologyManager(SOMA_ONTOLOGY_IRI) @@ -72,7 +72,7 @@ def setUpClass(cls): RobotDescription.current_robot_description.name + cls.extension) cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False cls.viz_marker_publisher = VizMarkerPublisher() diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 55ce2bb51..21380cd19 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -53,8 +53,7 @@ def test_remove_object(self): self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", - ObjectDescription, pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) def test_remove_robot(self): robot_id = self.robot.id diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index caef9dad4..190a48922 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -33,8 +33,8 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, RobotDescription.current_robot_description.name + extension, - ObjectDescription) + cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + RobotDescription.current_robot_description.name + extension) ProcessModule.execution_delay = True def setUp(self): diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 01e9920ee..3d5a4d7e0 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -55,7 +55,7 @@ def tearDown(self): self.multiverse.reset_world_and_remove_objects() def test_spawn_mesh_object(self): - milk = Object("milk", ObjectType.MILK, "milk.stl", description=MJCF, pose=Pose([1, 1, 0.1])) + milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) self.multiverse.simulate(0.2) contact_points = milk.contact_points() @@ -300,8 +300,8 @@ 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.1]) - # This is needed because the cup is spawned in the air so it needs to fall + cup = self.spawn_cup([1, 1, 0.2]) + # 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.2) contact_points = self.multiverse.get_object_contact_points(cup) diff --git a/test/test_object.py b/test/test_object.py index 74c22f7b8..1c128fa8f 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -5,15 +5,17 @@ from pycram.datastructures.enums import JointType, ObjectType from pycram.datastructures.pose import Pose from pycram.datastructures.dataclasses import Color +from pycram.failures import UnsupportedFileExtension from pycram.world_concepts.world_object import Object from geometry_msgs.msg import Point, Quaternion import pathlib + class TestObject(BulletWorldTestCase): def test_wrong_object_description_path(self): - with self.assertRaises(FileNotFoundError): + with self.assertRaises(UnsupportedFileExtension): milk = Object("milk_not_found", ObjectType.MILK, "wrong_path.sk") def test_malformed_object_description(self): From 1bd1b52c6357a3d22bebdb0faf103cb0ba1c36b5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 16:02:12 +0200 Subject: [PATCH 155/189] [MultiverseMJCF] changed variable name. --- src/pycram/object_descriptors/mjcf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 22c19d343..588bed48b 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -318,8 +318,8 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = factory.export_to_mjcf(output_file_path=save_path) def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: - mjcf_string = mjcf.from_file(path) - self.write_description_to_file(mjcf_string, save_path) + mjcf_model = mjcf.from_file(path) + self.write_description_to_file(mjcf_model, save_path) def generate_from_parameter_server(self, name: str, save_path: str) -> None: mjcf_string = rospy.get_param(name) From 4016d93cd7070fd0967358be9a79bc3ef9dd1b70 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 16:24:03 +0200 Subject: [PATCH 156/189] [MultiverseInterface] merged dev. --- src/pycram/object_descriptors/generic.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py index b30b224e9..0423f97e5 100644 --- a/src/pycram/object_descriptors/generic.py +++ b/src/pycram/object_descriptors/generic.py @@ -1,3 +1,5 @@ +from typing import Optional + from typing_extensions import List, Any, Union, Dict from geometry_msgs.msg import Point @@ -104,6 +106,11 @@ def generate_from_description_file(cls, path: str) -> str: def generate_from_parameter_server(cls, name: str) -> str: raise NotImplementedError + def add_joint(self, name: str, child: str, joint_type: JointType, + axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + ... + @property def shape_data(self) -> List[float]: return self._links[0].geometry.shape_data()['halfExtents'] From c5bd732d40863e128aaf4a30edde9b806749bfa0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 16:50:42 +0200 Subject: [PATCH 157/189] [MultiverseController] merged dev and fixed issues. --- src/pycram/datastructures/world.py | 7 +++++ src/pycram/object_descriptors/generic.py | 37 ++++++++++++++++++++--- src/pycram/world_concepts/world_object.py | 12 ++++---- 3 files changed, 45 insertions(+), 11 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 7e7769429..9ab81caab 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -37,6 +37,7 @@ if TYPE_CHECKING: from ..world_concepts.world_object import Object from ..description import Link, Joint, ObjectDescription + from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription class World(StateEntity, ABC): @@ -323,6 +324,12 @@ def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose """ pass + def load_generic_object_and_get_id(self, description: GenericObjectDescription) -> int: + """ + Creates a visual and collision box in the simulation and returns the id of the loaded object. + """ + raise NotImplementedError + def get_object_names(self) -> List[str]: """ Returns the names of all objects in the World. diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py index 0423f97e5..9e94b645d 100644 --- a/src/pycram/object_descriptors/generic.py +++ b/src/pycram/object_descriptors/generic.py @@ -1,4 +1,4 @@ -from typing import Optional +from typing import Optional, Tuple from typing_extensions import List, Any, Union, Dict @@ -13,7 +13,8 @@ class LinkDescription(AbstractLinkDescription): - def __init__(self, name: str, visual_frame_position: List[float], half_extents: List[float], color: Color = Color()): + def __init__(self, name: str, visual_frame_position: List[float], half_extents: List[float], + color: Color = Color()): self.parsed_description: BoxVisualShape = BoxVisualShape(color, visual_frame_position, half_extents) self._name: str = name @@ -36,6 +37,14 @@ def color(self) -> Color: class JointDescription(AbstractJointDescription): + @property + def parent(self) -> str: + raise NotImplementedError + + @property + def child(self) -> str: + raise NotImplementedError + @property def type(self) -> JointType: return JointType.UNKNOWN @@ -99,16 +108,33 @@ def generate_from_mesh_file(cls, path: str, name: str) -> str: raise NotImplementedError @classmethod - def generate_from_description_file(cls, path: str) -> str: + def generate_from_description_file(cls, path: str, make_mesh_paths_absolute: bool = True) -> str: raise NotImplementedError @classmethod def generate_from_parameter_server(cls, name: str) -> str: raise NotImplementedError + @property + def parent_map(self) -> Dict[str, Tuple[str, str]]: + return {} + + @property + def link_map(self) -> Dict[str, LinkDescription]: + return {self._links[0].name: self._links[0]} + + @property + def joint_map(self) -> Dict[str, JointDescription]: + return {} + + @property + def child_map(self) -> Dict[str, List[Tuple[str, str]]]: + return {} + def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, - lower_limit: Optional[float] = None, upper_limit: Optional[float] = None) -> None: + lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, + is_virtual: Optional[bool] = False) -> None: ... @property @@ -137,7 +163,8 @@ def get_joint_by_name(self, joint_name: str) -> JointDescription: def get_root(self) -> str: return self._links[0].name - def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]: + def get_chain(self, start_link_name: str, end_link_name: str, joints: Optional[bool] = True, + links: Optional[bool] = True, fixed: Optional[bool] = True) -> List[str]: raise NotImplementedError("Do Not Do This on generic objects as they have no chains") @staticmethod diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9e066f305..8d94b8c49 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -65,6 +65,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, pose = Pose() if pose is None else pose self.name: str = name + self.path: Optional[str] = path self.obj_type: ObjectType = obj_type self.color: Color = color self.description = description() @@ -74,10 +75,11 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose - self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, - self.description, self.name) + if path is not None: + self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, + self.description, self.name) - self.description.update_description_from_file(self.path) + self.description.update_description_from_file(self.path) if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: self._update_world_robot_and_description() @@ -210,9 +212,7 @@ def _spawn_object_and_get_id(self) -> int: :return: The unique id of the object and the path of the file that was loaded. """ if isinstance(self.description, GenericObjectDescription): - return self.world.load_generic_object_and_get_id(self.description), path - - self.path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self) + return self.world.load_generic_object_and_get_id(self.description) path = self.path if self.world.let_pycram_handle_spawning else self.name From 8776b99a9ef550afc5e75047590eb2a2a2e0537a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 16:52:48 +0200 Subject: [PATCH 158/189] [MultiverseController] fixed incorrect enum value. --- src/pycram/process_module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index a1ee38f19..eca953db6 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -220,7 +220,7 @@ def plan(): def wrapper(*args, **kwargs): pre = ProcessModuleManager.execution_type - ProcessModuleManager.execution_type = ExecutionType.Simulated + ProcessModuleManager.execution_type = ExecutionType.SIMULATED ret = func(*args, **kwargs) ProcessModuleManager.execution_type = pre return ret From 128522746199157d45b505e6924d4d55be77e90c Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 17:34:08 +0200 Subject: [PATCH 159/189] [MultiverseActions] merged new dev changes and fixed issues. --- src/pycram/world_concepts/world_object.py | 6 +++--- src/pycram/worlds/bullet_world.py | 6 +++--- test/bullet_world_testcase.py | 2 +- test/test_bullet_world.py | 3 +-- test/test_failure_handling.py | 4 ++-- test/test_object.py | 5 +++-- test/test_robot_description.py | 6 +++--- 7 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 05f81829d..2049746ad 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -37,7 +37,7 @@ class Object(WorldEntity): """ def __init__(self, name: str, obj_type: ObjectType, path: str, - description: Type[ObjectDescription] = URDFObject, + description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, color: Color = Color(), @@ -68,7 +68,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.path: Optional[str] = path self.obj_type: ObjectType = obj_type self.color: Color = color - self.description = description() + self.description = description if description is not None else URDFObject() self.cache_manager = self.world.cache_manager self.local_transformer = LocalTransformer() @@ -1347,7 +1347,7 @@ def copy_to_world(self, world: World) -> Object: :param world: The world to which the object should be copied. :return: The copied object in the given world. """ - obj = Object(self.name, self.obj_type, self.path, type(self.description), self.get_pose(), + obj = Object(self.name, self.obj_type, self.path, self.description, self.get_pose(), world, self.color) return obj diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index c8a87bf0e..a6cd378e6 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -78,15 +78,15 @@ def load_generic_object_and_get_id(self, description: GenericObjectDescription) """ # Create visual shape vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=description.shape_data, - rgbaColor=description.color.get_rgba()) + rgbaColor=description.color.get_rgba(), physicsClientId=self.id) # Create collision shape - col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=description.shape_data) + col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=description.shape_data, physicsClientId=self.id) # Create MultiBody with both visual and collision shapes obj_id = p.createMultiBody(baseMass=1.0, baseCollisionShapeIndex=col_shape, baseVisualShapeIndex=vis_shape, basePosition=description.origin.position_as_list(), - baseOrientation=description.origin.orientation_as_list()) + baseOrientation=description.origin.orientation_as_list(), physicsClientId=self.id) # Assuming you have a list to keep track of created objects return obj_id diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index 7e572500b..0bccf9316 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -30,7 +30,7 @@ def setUpClass(cls): RobotDescription.current_robot_description.name + cls.extension) cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - ObjectDescription, pose=Pose([1.3, 0.7, 0.95])) + pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False cls.viz_marker_publisher = VizMarkerPublisher() OntologyManager(SOMA_ONTOLOGY_IRI) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index cd6990b5a..65cdd9178 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -53,8 +53,7 @@ def test_remove_object(self): self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", - ObjectDescription, pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) def test_remove_robot(self): robot_id = self.robot.id diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index caef9dad4..190a48922 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -33,8 +33,8 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, RobotDescription.current_robot_description.name + extension, - ObjectDescription) + cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + RobotDescription.current_robot_description.name + extension) ProcessModule.execution_delay = True def setUp(self): diff --git a/test/test_object.py b/test/test_object.py index 5142aa85d..4b5ecf82e 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -166,6 +166,7 @@ def test_object_equal(self): class GenericObjectTestCase(BulletWorldTestCase): def test_init_generic_object(self): - gen_obj_desc = lambda: GenericObjectDescription("robokudo_object", [0,0,0], [0.1, 0.1, 0.1]) + gen_obj_desc = GenericObjectDescription("robokudo_object", [0,0,0], [0.1, 0.1, 0.1]) obj = Object("robokudo_object", ObjectType.MILK, None, gen_obj_desc) - self.assertTrue(True) + pose = obj.get_pose() + self.assertTrue(isinstance(pose, Pose)) diff --git a/test/test_robot_description.py b/test/test_robot_description.py index d30366f45..e845d9ec6 100644 --- a/test/test_robot_description.py +++ b/test/test_robot_description.py @@ -3,7 +3,7 @@ from pycram.robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ CameraDescription, RobotDescriptionManager from pycram.datastructures.enums import Arms, GripperState -from pycram.object_descriptors.urdf import ObjectDescription as URDFObject +from pycram.object_descriptors.urdf import ObjectDescription as URDF class TestRobotDescription(unittest.TestCase): @@ -12,7 +12,7 @@ class TestRobotDescription(unittest.TestCase): def setUpClass(cls): cls.path = str(pathlib.Path(__file__).parent.resolve()) + '/../resources/robots/' + "pr2" + '.urdf' cls.path_turtlebot = str(pathlib.Path(__file__).parent.resolve()) + '/../resources/robots/' + "turtlebot" + '.urdf' - cls.urdf_obj = URDFObject(cls.path) + cls.urdf_obj = URDF(cls.path) def test_robot_description_construct(self): robot_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", self.path) @@ -20,7 +20,7 @@ def test_robot_description_construct(self): self.assertEqual(robot_description.base_link, "base_link") self.assertEqual(robot_description.torso_link, "torso_lift_link") self.assertEqual(robot_description.torso_joint, "torso_lift_joint") - self.assertTrue(type(robot_description.urdf_object) is URDFObject) + self.assertTrue(type(robot_description.urdf_object) is URDF) self.assertEqual(len(robot_description.links), 88) self.assertEqual(len(robot_description.joints), 87) From 64774842990076b7b393dfacd4e19d2a38b71289 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 17:39:02 +0200 Subject: [PATCH 160/189] [MultiverseActions] fixed contact test. --- test/test_multiverse.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4849e0b5f..4a30ff6a2 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import unittest +from time import sleep import numpy as np import psutil @@ -286,13 +287,13 @@ def test_attach_with_robot(self): def test_get_object_contact_points(self): for i in range(3): - milk = self.spawn_milk([1, 1, 0], [0, -0.707, 0, 0.707]) + milk = self.spawn_milk([1, 1, 0.02], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) 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.1]) + cup = self.spawn_cup([1, 1, 0.2]) # 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) @@ -301,7 +302,9 @@ 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, milk) + sleep(0.1) self.tearDown() + sleep(0.1) def test_get_contact_points_between_two_objects(self): for i in range(3): From 3ad907927b897af3761343ee072b5dff7e8f4f0e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 17:52:14 +0200 Subject: [PATCH 161/189] [MultiverseActions] fixed contact test. --- src/pycram/worlds/multiverse_communication/clients.py | 7 ++++++- test/test_multiverse.py | 6 +++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index dfa740f18..a8ebff101 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -3,6 +3,7 @@ import threading from time import time, sleep +import rospy from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData @@ -712,7 +713,11 @@ def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: :param contact_effort: The contact effort of the object as a list of strings. :return: The contact effort of the object as a list of floats. """ - return list(map(float, contact_effort[0].split())) + contact_effort = contact_effort[0].split() + if contact_effort == 'failed': + rospy.logwarn("Failed to get contact effort") + return [0.0] * 6 + return list(map(float, contact_effort)) def _get_contact_points(self, object_name) -> Dict[API, List]: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 1266dc879..50411fe62 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -315,9 +315,11 @@ def test_get_object_contact_points(self): sleep(0.1) def test_get_contact_points_between_two_objects(self): + sleep(0.1) 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.1]) + sleep(0.1) + cup = self.spawn_cup([1, 1, 0.2]) # 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.2) @@ -327,7 +329,9 @@ def test_get_contact_points_between_two_objects(self): self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_a.object, milk) self.assertTrue(contact_points[0].link_b.object, cup) + sleep(0.1) self.tearDown() + sleep(0.1) def test_get_one_ray(self): milk = self.spawn_milk([1, 1, 0.1]) From e540547307be4049fdffe1b4b60f6562c8b1ef53 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 22 Aug 2024 22:34:23 +0200 Subject: [PATCH 162/189] [MultiverseMJCF] Do not restore the state from the physics simultor, instead restore it manually (due to issues with pybullet restore api) Costmaps uses world api not the pybullet api for visualizing. Fixed multiverse reset world issue. Update the latest saved state whenever an object is removed or added since it has to have the same objects. Use add_object method of the world instead of appending to the objects list directly. Reset world now uses the state equalization to the original_state instead of looping over all object poses and joints (thus same as in world_sync) --- src/pycram/costmaps.py | 47 ++--- src/pycram/datastructures/dataclasses.py | 17 ++ src/pycram/datastructures/world.py | 177 ++++++++++++++++-- src/pycram/description.py | 1 + src/pycram/designators/location_designator.py | 1 + src/pycram/world_concepts/world_object.py | 4 +- src/pycram/worlds/bullet_world.py | 23 ++- src/pycram/worlds/multiverse.py | 2 +- .../multiverse_communication/clients.py | 18 +- test/bullet_world_testcase.py | 4 +- test/test_bullet_world.py | 2 +- test/test_multiverse.py | 45 ++--- 12 files changed, 241 insertions(+), 100 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 43938ffcd..f59bdfe07 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -19,8 +19,7 @@ from .datastructures.pose import Pose, Transform from .datastructures.world import World from .datastructures.dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color - -import pybullet as p +from tf.transformations import quaternion_from_matrix @dataclass @@ -111,42 +110,21 @@ def visualize(self) -> None: # Creation of the visual shapes, for documentation of the visual shapes # please look here: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf for box in boxes: - visual = p.createVisualShape(p.GEOM_BOX, - halfExtents=[(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, - 0.001], - rgbaColor=[1, 0, 0, 0.6], - visualFramePosition=[(box[0][0] + box[1] / 2) * self.resolution, - (box[0][1] + box[2] / 2) * self.resolution, 0.]) + box = BoxVisualShape(Color(1, 0, 0, 0.6), + [(box[0][0] + box[1] / 2) * self.resolution, + (box[0][1] + box[2] / 2) * self.resolution, 0.], + [(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, 0.001]) + visual = self.world.create_visual_shape(box) cells.append(visual) # Set to 127 for since this is the maximal amount of links in a multibody for cell_parts in self._chunks(cells, 127): - # Dummy paramater since these are needed to spawn visual shapes as a - # multibody. - link_poses = [[0, 0, 0] for c in cell_parts] - link_orientations = [[0, 0, 0, 1] for c in cell_parts] - link_masses = [1.0 for c in cell_parts] - link_parent = [0 for c in cell_parts] - link_joints = [p.JOINT_FIXED for c in cell_parts] - link_collision = [-1 for c in cell_parts] - link_joint_axis = [[1, 0, 0] for c in cell_parts] - # The position at which the multibody will be spawned. Offset such that - # the origin referes to the centre of the costmap. - # origin_pose = self.origin.position_as_list() - # base_pose = [origin_pose[0] - self.height / 2 * self.resolution, - # origin_pose[1] - self.width / 2 * self.resolution, origin_pose[2]] - offset = [[-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05], [0, 0, 0, 1]] - new_pose = p.multiplyTransforms(self.origin.position_as_list(), self.origin.orientation_as_list(), - offset[0], offset[1]) - - map_obj = p.createMultiBody(baseVisualShapeIndex=-1, linkVisualShapeIndices=cell_parts, - basePosition=new_pose[0], baseOrientation=new_pose[1], linkPositions=link_poses, - # [0, 0, 1, 0] - linkMasses=link_masses, linkOrientations=link_orientations, - linkInertialFramePositions=link_poses, - linkInertialFrameOrientations=link_orientations, linkParentIndices=link_parent, - linkJointTypes=link_joints, linkJointAxis=link_joint_axis, - linkCollisionShapeIndices=link_collision) + origin_transform = (Transform(self.origin.position_as_list(), self.origin.orientation_as_list()) + .get_homogeneous_matrix()) + offset_transform = (Transform(offset[0], offset[1]).get_homogeneous_matrix()) + new_pose_transform = np.dot(origin_transform, offset_transform) + new_pose = Pose(new_pose_transform[:3, 3].tolist(), quaternion_from_matrix(new_pose_transform)) + map_obj = self.world.create_multi_body_from_visual_shapes(cell_parts, new_pose) self.vis_ids.append(map_obj) def _chunks(self, lst: List, n: int) -> List: @@ -461,7 +439,6 @@ def _create_from_world(self, size: int, resolution: float) -> np.ndarray: i = 0 j = 0 for n in self._chunks(np.array(rays), 16380): - # with UseProspectionWorld(): r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) while r_t is None: r_t = World.current_world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index ef890d603..73ef87c9b 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -1,6 +1,7 @@ from __future__ import annotations from abc import ABC, abstractmethod +from copy import deepcopy, copy from dataclasses import dataclass from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING @@ -335,6 +336,9 @@ def all_constraints_are_equal(self, other: 'LinkState'): return all([cid == other_cid for cid, other_cid in zip(self.constraint_ids.values(), other.constraint_ids.values())]) + def __copy__(self): + return LinkState(constraint_ids=copy(self.constraint_ids)) + @dataclass class JointState(State): @@ -348,6 +352,9 @@ def __eq__(self, other: 'JointState'): error = calculate_joint_position_error(self.position, other.position) return is_error_acceptable(error, other.acceptable_error) + def __copy__(self): + return JointState(position=self.position, acceptable_error=self.acceptable_error) + @dataclass class ObjectState(State): @@ -391,6 +398,12 @@ def all_attachments_are_equal(self, other: 'ObjectState'): """ return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) + def __copy__(self): + return ObjectState(pose=self.pose.copy(), attachments=copy(self.attachments), + link_states=copy(self.link_states), + joint_states=copy(self.joint_states), + acceptable_pose_error=deepcopy(self.acceptable_pose_error)) + @dataclass class WorldState(State): @@ -431,6 +444,10 @@ def all_objects_states_are_equal(self, other: 'WorldState'): for obj_state, other_obj_state in zip(self.object_states.values(), other.object_states.values())]) + def __copy__(self): + return WorldState(simulator_state_id=self.simulator_state_id, + object_states=deepcopy(self.object_states)) + @dataclass class LateralFriction: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 0afaac03f..777a2193c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -163,6 +163,20 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self._init_goal_validators() + self.original_state_id = self.save_state() + + def add_object(self, obj: Object) -> None: + """ + Adds an object to the world. + + :param obj: The object to be added. + """ + self.object_lock.acquire() + self.objects.append(obj) + self.object_lock.release() + latest_state: WorldState = self._saved_states[self.original_state_id] + latest_state.object_states[obj.name] = obj.current_state + @property def robot_description(self) -> RobotDescription: """ @@ -355,8 +369,22 @@ def get_object_by_id(self, obj_id: int) -> Object: """ return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] - @abstractmethod def remove_object_by_id(self, obj_id: int) -> bool: + """ + Removes the object with the given id from the world, and saves a new original state for the world. + + :param obj_id: The unique id of the object to be removed. + :return: Whether the object was removed successfully. + """ + removed = self._remove_object_by_id(obj_id) + if removed: + self.original_state_id = self.save_state() + else: + rospy.logwarn(f"Object with id {obj_id} could not be removed.") + return removed + + @abstractmethod + def _remove_object_by_id(self, obj_id: int) -> bool: """ Removes the object with the given id from the world. @@ -390,6 +418,8 @@ def remove_object(self, obj: Object) -> None: if self.remove_object_from_simulator(obj): self.objects.remove(obj) + latest_state: WorldState = self._saved_states[self.original_state_id] + latest_state.object_states.pop(obj.name) if World.robot == obj: World.robot = None @@ -847,12 +877,15 @@ def robot_is_set() -> bool: """ return World.robot is not None - def exit(self) -> None: + def exit(self, remove_saved_states: bool = True) -> None: """ Closes the World as well as the prospection world, also collects any other thread that is running. + + :param remove_saved_states: Whether to remove the saved states. """ self.exit_prospection_world_if_exists() - self.reset_world_and_remove_objects() + self.reset_world(remove_saved_states) + self.remove_all_objects() self.disconnect_from_physics_server() self.reset_robot() self.join_threads() @@ -924,7 +957,6 @@ def current_state(self) -> WorldState: @current_state.setter def current_state(self, state: WorldState) -> None: if self.current_state != state: - self.restore_physics_simulator_state(state.simulator_state_id) for obj in self.objects: self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] @@ -1078,17 +1110,17 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec """ return self.world_sync.get_world_object(prospection_object) - def reset_world_and_remove_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: + def remove_all_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: """ - Resets the World to the state it was first spawned in and removes all objects from the World. + Removes all objects from the World. + :param exclude_objects: A list of objects that should not be removed. """ - self.reset_world() objs_copy = [obj for obj in self.objects] exclude_objects = [] if exclude_objects is None else exclude_objects [self.remove_object(obj) for obj in objs_copy if obj not in exclude_objects] - def reset_world(self, remove_saved_states=True) -> None: + def reset_world(self, remove_saved_states=False) -> None: """ Resets the World to the state it was first spawned in. All attached objects will be detached, all joints will be set to the @@ -1097,12 +1129,10 @@ def reset_world(self, remove_saved_states=True) -> None: :param remove_saved_states: If the saved states should be removed. """ - + self.restore_state(self.original_state_id) if remove_saved_states: self.remove_saved_states() - - for obj in self.objects: - obj.reset(remove_saved_states) + self.original_state_id = self.save_state() def remove_saved_states(self) -> None: """ @@ -1111,6 +1141,9 @@ def remove_saved_states(self) -> None: for state_id in self.saved_states: self.remove_physics_simulator_state(state_id) super().remove_saved_states() + for obj in self.objects: + obj.remove_saved_states() + self.original_state_id = None def update_transforms_for_objects_in_current_world(self) -> None: """ @@ -1151,6 +1184,12 @@ def create_visual_shape(self, visual_shape: VisualShape) -> int: :param visual_shape: The visual shape to be created, uses the VisualShape dataclass defined in world_dataclasses :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_visual_shape, visual_shape) + + def _create_visual_shape(self, visual_shape: VisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_visual_shape` + """ raise NotImplementedError def create_multi_body_from_visual_shapes(self, visual_shape_ids: List[int], pose: Pose) -> int: @@ -1188,6 +1227,12 @@ def create_multi_body(self, multi_body: MultiBody) -> int: :param multi_body: The multi body to be created, uses the MultiBody dataclass defined in world_dataclasses. :return: The unique id of the created multi body. """ + return self._simulator_object_creator(self._create_multi_body, multi_body) + + def _create_multi_body(self, multi_body: MultiBody) -> int: + """ + See :py:meth:`~pycram.world.World.create_multi_body` + """ raise NotImplementedError def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: @@ -1198,6 +1243,12 @@ def create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_box_visual_shape, shape_data) + + def _create_box_visual_shape(self, shape_data: BoxVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_box_visual_shape` + """ raise NotImplementedError def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: @@ -1208,6 +1259,12 @@ def create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: CylinderVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_cylinder_visual_shape, shape_data) + + def _create_cylinder_visual_shape(self, shape_data: CylinderVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_cylinder_visual_shape` + """ raise NotImplementedError def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: @@ -1218,6 +1275,12 @@ def create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_sphere_visual_shape, shape_data) + + def _create_sphere_visual_shape(self, shape_data: SphereVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_sphere_visual_shape` + """ raise NotImplementedError def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: @@ -1228,6 +1291,12 @@ def create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: CapsuleVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_capsule_visual_shape, shape_data) + + def _create_capsule_visual_shape(self, shape_data: CapsuleVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_capsule_visual_shape` + """ raise NotImplementedError def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: @@ -1238,6 +1307,12 @@ def create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_plane_visual_shape, shape_data) + + def _create_plane_visual_shape(self, shape_data: PlaneVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_plane_visual_shape` + """ raise NotImplementedError def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: @@ -1248,6 +1323,12 @@ def create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: uses the MeshVisualShape dataclass defined in world_dataclasses. :return: The unique id of the created shape. """ + return self._simulator_object_creator(self._create_mesh_visual_shape, shape_data) + + def _create_mesh_visual_shape(self, shape_data: MeshVisualShape) -> int: + """ + See :py:meth:`~pycram.world.World.create_mesh_visual_shape` + """ raise NotImplementedError def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, @@ -1269,7 +1350,16 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ :param parent_link_id: The id of the link to which the text should be attached. :return: The id of the added text. """ - rospy.logwarn(f"add_text is not implemented in {self.__class__.__name__}") + return self._simulator_object_creator(self._add_text, text, position, orientation, size, color, life_time, + parent_object_id, parent_link_id) + + def _add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: float = 0.1, + color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: + """ + See :py:meth:`~pycram.world.World.add_text` + """ + raise NotImplementedError def remove_text(self, text_id: Optional[int] = None) -> None: """ @@ -1277,7 +1367,13 @@ def remove_text(self, text_id: Optional[int] = None) -> None: :param text_id: The id of the text to be removed. """ - rospy.logwarn(f"remove_text is not implemented in {self.__class__.__name__}") + self._simulator_object_remover(self._remove_text, text_id) + + def _remove_text(self, text_id: Optional[int] = None) -> None: + """ + See :py:meth:`~pycram.world.World.remove_text` + """ + raise NotImplementedError def enable_joint_force_torque_sensor(self, obj: Object, fts_joint_idx: int) -> None: """ @@ -1333,11 +1429,58 @@ def resume_world_sync(self) -> None: """ self.world_sync.sync_lock.release() - def add_vis_axis(self, pose: Pose) -> None: - rospy.logwarn(f"add_vis_axis is not implemented in {self.__class__.__name__}") + def add_vis_axis(self, pose: Pose) -> int: + """ + Adds a visual axis to the world. + + :param pose: The pose of the visual axis. + :return: The id of the added visual axis. + """ + return self._simulator_object_creator(self._add_vis_axis, pose) + + def _add_vis_axis(self, pose: Pose) -> None: + """ + See :py:meth:`~pycram.world.World.add_vis_axis` + """ + raise NotImplementedError def remove_vis_axis(self) -> None: - rospy.logwarn(f"remove_vis_axis is not implemented in {self.__class__.__name__}") + """ + Removes the visual axis from the world. + """ + self._simulator_object_remover(self._remove_vis_axis) + + def _remove_vis_axis(self) -> None: + """ + See :py:meth:`~pycram.world.World.remove_vis_axis` + """ + raise NotImplementedError + + def _simulator_object_creator(self, creator_func: Callable, *args, **kwargs) -> int: + """ + Creates an object in the physics simulator and returns the created object id. + + :param creator_func: The function that creates the object in the physics simulator. + :param args: The arguments for the creator function. + :param kwargs: The keyword arguments for the creator function. + :return: The created object id. + """ + obj_id = creator_func(*args, **kwargs) + latest_state: WorldState = self._saved_states[self.original_state_id] + latest_state.simulator_state_id = self.save_physics_simulator_state() + return obj_id + + def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) -> None: + """ + Removes an object from the physics simulator. + + :param remover_func: The function that removes the object from the physics simulator. + :param args: The arguments for the remover function. + :param kwargs: The keyword arguments for the remover function. + """ + remover_func(*args, **kwargs) + latest_state: WorldState = self._saved_states[self.original_state_id] + latest_state.simulator_state_id = self.save_physics_simulator_state() def __del__(self): self.exit() diff --git a/src/pycram/description.py b/src/pycram/description.py index 82143a994..f99cb58e4 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -213,6 +213,7 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): LinkDescription.__init__(self, link_description.parsed_description) self.local_transformer: LocalTransformer = LocalTransformer() self.constraint_ids: Dict[Link, int] = {} + self._current_pose: Optional[Pose] = None self.update_pose() def set_pose(self, pose: Pose) -> None: diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index c3462bc56..5bd50bcfb 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -178,6 +178,7 @@ def __iter__(self): if self.visible_for or self.reachable_for: robot_object = self.visible_for.world_object if self.visible_for else self.reachable_for.world_object test_robot = World.current_world.get_prospection_object_for_object(robot_object) + with UseProspectionWorld(): for maybe_pose in PoseGenerator(final_map, number_of_samples=600): res = True diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 093778edb..ffb64193f 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -105,7 +105,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.attachments: Dict[Object, Attachment] = {} - self.world.objects.append(self) + self.world.add_object(self) def _resolve_description(self, path: Optional[str] = None, description: Optional[ObjectDescription] = None) -> None: """ @@ -575,7 +575,7 @@ def remove(self) -> None: """ self.world.remove_object(self) - def reset(self, remove_saved_states=True) -> None: + def reset(self, remove_saved_states=False) -> None: """ Reset the Object to the state it was first spawned in. All attached objects will be detached, all joints will be set to the diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 010b29d65..385d6b9c5 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -103,9 +103,9 @@ def _load_object_and_get_id(self, path: str, pose: Pose) -> int: baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) def remove_object_from_simulator(self, obj: Object) -> bool: - return self.remove_object_by_id(obj.id) + return self._remove_object_by_id(obj.id) - def remove_object_by_id(self, obj_id: int) -> True: + def _remove_object_by_id(self, obj_id: int) -> True: p.removeBody(obj_id, self.id) return True @@ -298,14 +298,15 @@ def restore_physics_simulator_state(self, state_id): def remove_physics_simulator_state(self, state_id: int): p.removeState(state_id, physicsClientId=self.id) - def add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> None: + def _add_vis_axis(self, pose: Pose, + length: Optional[float] = 0.2) -> int: """ Creates a Visual object which represents the coordinate frame at the given position and orientation. There can be an unlimited amount of vis axis objects. :param pose: The pose at which the axis should be spawned :param length: Optional parameter to configure the length of the axes + :return: The id of the spawned object """ pose_in_map = self.local_transformer.transform_pose(pose, "map") @@ -327,9 +328,11 @@ def add_vis_axis(self, pose: Pose, link_joint_axis=[Point(1, 0, 0), Point(0, 1, 0), Point(0, 0, 1)], link_collision_shape_indices=[-1, -1, -1]) - self.vis_axis.append(self.create_multi_body(multibody)) + body_id = self._create_multi_body(multibody) + self.vis_axis.append(body_id) + return body_id - def remove_vis_axis(self) -> None: + def _remove_vis_axis(self) -> None: """ Removes all spawned vis axis objects that are currently in this BulletWorld. """ @@ -346,13 +349,13 @@ def ray_test_batch(self, from_positions: List[List[float]], to_positions: List[L return p.rayTestBatch(from_positions, to_positions, numThreads=num_threads, physicsClientId=self.id) - def create_visual_shape(self, visual_shape: VisualShape) -> int: + def _create_visual_shape(self, visual_shape: VisualShape) -> int: return p.createVisualShape(visual_shape.visual_geometry_type.value, rgbaColor=visual_shape.rgba_color.get_rgba(), visualFramePosition=visual_shape.visual_frame_position, physicsClientId=self.id, **visual_shape.shape_data()) - def create_multi_body(self, multi_body: MultiBody) -> int: + def _create_multi_body(self, multi_body: MultiBody) -> int: return p.createMultiBody(baseVisualShapeIndex=-multi_body.base_visual_shape_index, linkVisualShapeIndices=multi_body.link_visual_shape_indices, basePosition=multi_body.base_pose.position_as_list(), @@ -385,7 +388,7 @@ def get_images_for_target(self, return list(p.getCameraImage(size, size, view_matrix, projection_matrix, physicsClientId=self.id))[2:5] - def add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, + def _add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: args = {} @@ -401,7 +404,7 @@ def add_text(self, text: str, position: List[float], orientation: Optional[List[ args["parentLinkIndex"] = parent_link_id return p.addUserDebugText(text, position, color.get_rgb(), physicsClientId=self.id, **args) - def remove_text(self, text_id: Optional[int] = None) -> None: + def _remove_text(self, text_id: Optional[int] = None) -> None: if text_id is not None: p.removeUserDebugItem(text_id, physicsClientId=self.id) else: diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index e73727aac..c1687741c 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -485,7 +485,7 @@ def join_threads(self) -> None: self.reader.stop_thread = True self.reader.join() - def remove_object_by_id(self, obj_id: int) -> bool: + def _remove_object_by_id(self, obj_id: int) -> bool: obj = self.get_object_by_id(obj_id) return self.remove_object_from_simulator(obj) diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index a8ebff101..3313b74ac 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -346,16 +346,18 @@ def spawn_robot_with_actuators(self, robot_name: str, position: List[float], ori data = [self.sim_time, *position, *orientation, *relative_velocity] self.send_data_to_server(data, send_meta_data=send_meta_data, receive_meta_data=actuator_joint_commands) - def _reset_request_meta_data(self): + def _reset_request_meta_data(self, set_simulation_name: bool = True): """ Reset the request metadata. + + :param set_simulation_name: Whether to set the simulation name to the value of self.simulation_name. """ self.request_meta_data = { "meta_data": self._meta_data.__dict__.copy(), "send": {}, "receive": {}, } - if self.simulation is not None: + if self.simulation is not None and set_simulation_name: self.request_meta_data["meta_data"]["simulation_name"] = self.simulation def set_body_pose(self, body_name: str, position: List[float], orientation: List[float]) -> None: @@ -421,7 +423,7 @@ def reset_world(self) -> None: """ Reset the world in the simulation. """ - self.send_data_to_server([0]) + self.send_data_to_server([0], set_simulation_name=False) def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, List[float]]) -> Dict: """ @@ -433,7 +435,7 @@ def send_body_data_to_server(self, body_name: str, body_data: Dict[Property, Lis """ send_meta_data = {body_name: list(map(str, body_data.keys()))} flattened_data = [value for data in body_data.values() for value in data] - return self.send_data_to_server([self.sim_time, *flattened_data], send_meta_data) + return self.send_data_to_server([self.sim_time, *flattened_data], send_meta_data=send_meta_data) def send_multiple_body_data_to_server(self, body_data: Dict[str, Dict[Property, List[float]]]) -> Dict: """ @@ -465,16 +467,18 @@ def send_meta_data_and_get_response(self, send_meta_data: Dict) -> Dict: def send_data_to_server(self, data: List, send_meta_data: Optional[Dict] = None, - receive_meta_data: Optional[Dict] = None) -> Dict: + receive_meta_data: Optional[Dict] = None, + set_simulation_name: bool = True) -> Dict: """ Send data to the multiverse server. :param data: The data to be sent. :param send_meta_data: The metadata to be sent. :param receive_meta_data: The metadata to be received. + :param set_simulation_name: Whether to set the simulation name to the value of self.simulation. :return: The response from the server. """ - self._reset_request_meta_data() + self._reset_request_meta_data(set_simulation_name=set_simulation_name) if send_meta_data: self.request_meta_data["send"] = send_meta_data if receive_meta_data: @@ -714,7 +718,7 @@ def _parse_constraint_effort(contact_effort: List[str]) -> List[float]: :return: The contact effort of the object as a list of floats. """ contact_effort = contact_effort[0].split() - if contact_effort == 'failed': + if 'failed' in contact_effort: rospy.logwarn("Failed to get contact effort") return [0.0] * 6 return list(map(float, contact_effort)) diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index dc6abc8c6..f37231711 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -36,7 +36,7 @@ def setUpClass(cls): OntologyManager(SOMA_ONTOLOGY_IRI) def setUp(self): - self.world.reset_world() + self.world.reset_world(remove_saved_states=True) with UseProspectionWorld(): pass @@ -47,7 +47,7 @@ def setUp(self): def tearDown(self): pycram.tasktree.reset_tree() time.sleep(0.05) - self.world.reset_world() + self.world.reset_world(remove_saved_states=True) with UseProspectionWorld(): pass diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 21380cd19..8aa57fd1a 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -64,7 +64,7 @@ def test_remove_robot(self): RobotDescription.current_robot_description.name + self.extension) def test_get_joint_position(self): - self.assertEqual(self.robot.get_joint_position("head_pan_joint"), 0.0) + self.assertAlmostEqual(self.robot.get_joint_position("head_pan_joint"), 0.0, delta=0.01) def test_get_object_contact_points(self): self.assertEqual(len(self.robot.contact_points()), 0) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 50411fe62..4b36f8f6a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -15,7 +15,6 @@ from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions from pycram.worlds.multiverse_extras.helpers import get_robot_mjcf_path, parse_mjcf_actuators -from pycram.object_descriptors.mjcf import ObjectDescription as MJCF multiverse_installed = True try: @@ -49,11 +48,11 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): - cls.multiverse.exit() + cls.multiverse.exit(remove_saved_states=True) cls.multiverse.remove_multiverse_resources() def tearDown(self): - self.multiverse.reset_world_and_remove_objects() + self.multiverse.remove_all_objects() def test_spawn_mesh_object(self): milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) @@ -93,9 +92,9 @@ def test_get_images_for_target(self): self.assertIsInstance(segmentation_mask, np.ndarray) self.assertTrue(depth.shape == (256, 256)) self.assertTrue(segmentation_mask.shape == (256, 256)) - self.assertAlmostEqual(np.max(depth), 0.5, delta=0.1) - self.assertTrue(np.unique(segmentation_mask).shape[0] == 2) self.assertTrue(milk.id in np.unique(segmentation_mask).flatten().tolist()) + avg_depth_of_milk = np.mean(depth[segmentation_mask == milk.id]) + self.assertAlmostEqual(avg_depth_of_milk, 0.5, delta=0.1) def test_reset_world(self): set_position = [1, 1, 0.1] @@ -245,19 +244,21 @@ def test_get_environment_pose(self): self.assertIsInstance(pose, Pose) def test_attach_object(self): - milk = self.spawn_milk([1, 0.1, 0.1]) - cup = self.spawn_cup([1, 1.1, 0.1]) - milk.attach(cup) - self.assertTrue(cup in milk.attachments) - milk_position = milk.get_position_as_list() - milk_position[0] += 1 - cup_position = cup.get_position_as_list() - estimated_cup_position = cup_position.copy() - estimated_cup_position[0] += 1 - milk.set_position(milk_position) - new_cup_position = cup.get_position_as_list() - self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], - self.multiverse.acceptable_position_error) + for _ in range(3): + milk = self.spawn_milk([1, 0.1, 0.1]) + cup = self.spawn_cup([1, 1.1, 0.1]) + milk.attach(cup) + self.assertTrue(cup in milk.attachments) + milk_position = milk.get_position_as_list() + milk_position[0] += 1 + cup_position = cup.get_position_as_list() + estimated_cup_position = cup_position.copy() + estimated_cup_position[0] += 1 + milk.set_position(milk_position) + new_cup_position = cup.get_position_as_list() + self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], + self.multiverse.acceptable_position_error) + self.tearDown() def test_detach_object(self): for i in range(2): @@ -294,7 +295,7 @@ def test_attach_with_robot(self): self.assert_poses_are_equal(milk_initial_pose, milk_pose) def test_get_object_contact_points(self): - for i in range(3): + for i in range(10): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) contact_points = self.multiverse.get_object_contact_points(milk) self.assertIsInstance(contact_points, ContactPointsList) @@ -310,15 +311,11 @@ 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, milk) - sleep(0.1) self.tearDown() - sleep(0.1) def test_get_contact_points_between_two_objects(self): - sleep(0.1) for i in range(3): milk = self.spawn_milk([1, 1, 0.01], [0, -0.707, 0, 0.707]) - sleep(0.1) cup = self.spawn_cup([1, 1, 0.2]) # This is needed because the cup is spawned in the air so it needs to fall # to get in contact with the milk @@ -329,9 +326,7 @@ def test_get_contact_points_between_two_objects(self): self.assertIsInstance(contact_points[0], ContactPoint) self.assertTrue(contact_points[0].link_a.object, milk) self.assertTrue(contact_points[0].link_b.object, cup) - sleep(0.1) self.tearDown() - sleep(0.1) def test_get_one_ray(self): milk = self.spawn_milk([1, 1, 0.1]) From 4db70f0ed4ac190153f5c92e8557b018221ce707 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 23 Aug 2024 12:14:11 +0200 Subject: [PATCH 163/189] [MultiverseMJCF] Handling the removing of the visualt object different from normal physical pycram object. Changed name of remove object by id to remove visual object as that what it was used for everywhere. --- src/pycram/costmaps.py | 2 +- src/pycram/datastructures/world.py | 188 +++++++++++++++++------------ src/pycram/worlds/bullet_world.py | 19 ++- src/pycram/worlds/multiverse.py | 6 +- 4 files changed, 128 insertions(+), 87 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index f59bdfe07..2c4aeab0f 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -143,7 +143,7 @@ def close_visualization(self) -> None: Removes the visualization from the World. """ for v_id in self.vis_ids: - self.world.remove_object_by_id(v_id) + self.world.remove_visual_object(v_id) self.vis_ids = [] def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int: diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 777a2193c..188878197 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -113,7 +113,7 @@ class World(StateEntity, ABC): def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, **config_kwargs): """ - Creates a new simulation, the mode decides if the simulation should be a rendered window or just run in the + Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. The World object also initializes the Events for attachment, detachment and for manipulating the world. @@ -167,27 +167,26 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ def add_object(self, obj: Object) -> None: """ - Adds an object to the world. + Add an object to the world. :param obj: The object to be added. """ self.object_lock.acquire() self.objects.append(obj) + self.add_object_to_original_state(obj) self.object_lock.release() - latest_state: WorldState = self._saved_states[self.original_state_id] - latest_state.object_states[obj.name] = obj.current_state @property def robot_description(self) -> RobotDescription: """ - Returns the current robot description. + Return the current robot description. """ return RobotDescription.current_robot_description @property def robot_has_actuators(self) -> bool: """ - Returns whether the robot has actuators. + Return whether the robot has actuators. """ return self.robot_description.has_actuators @@ -199,20 +198,20 @@ def get_actuator_for_joint(self, joint: Joint) -> str: def joint_has_actuator(self, joint: Joint) -> bool: """ - Returns whether the joint has an actuator. + Return whether the joint has an actuator. """ return joint.name in self.robot_joint_actuators @property def robot_joint_actuators(self) -> Dict[str, str]: """ - Returns the joint actuators of the robot. + Return the joint actuators of the robot. """ return self.robot_description.joint_actuators def _init_goal_validators(self): """ - Initializes the goal validators for the World objects' poses, positions, and orientations. + Initialize the goal validators for the World objects' poses, positions, and orientations. """ # Objects Pose goal validators @@ -236,7 +235,7 @@ def _init_goal_validators(self): def check_object_exists(self, obj: Object): """ - Checks if the object exists in the simulator and issues a warning if it does not. + Check if the object exists in the simulator and issues a warning if it does not. :param obj: The object to check. """ return obj not in self.objects @@ -244,13 +243,13 @@ def check_object_exists(self, obj: Object): @abstractmethod def _init_world(self, mode: WorldMode): """ - Initializes the physics simulation. + Initialize the physics simulation. """ raise NotImplementedError def _init_events(self): """ - Initializes dynamic events that can be used to react to changes in the World. + Initialize dynamic events that can be used to react to changes in the World. """ self.detachment_event: Event = Event() self.attachment_event: Event = Event() @@ -258,21 +257,21 @@ def _init_events(self): def _init_and_sync_prospection_world(self): """ - Initializes the prospection world and the synchronization between the main and the prospection world. + Initialize the prospection world and the synchronization between the main and the prospection world. """ self._init_prospection_world() self._sync_prospection_world() def _update_local_transformer_worlds(self): """ - Updates the local transformer worlds with the current world and prospection world. + Update the local transformer worlds with the current world and prospection world. """ self.local_transformer.world = self self.local_transformer.prospection_world = self.prospection_world def _init_prospection_world(self): """ - Initializes the prospection world, if this is a prospection world itself it will not create another prospection, + Initialize the prospection world, if this is a prospection world itself it will not create another prospection, world, but instead set the prospection world to None, else it will create a prospection world. """ if self.is_prospection_world: # then no need to add another prospection world @@ -284,7 +283,7 @@ def _init_prospection_world(self): def _sync_prospection_world(self): """ - Synchronizes the prospection world with the main world, this means that every object in the main world will be + Synchronize the prospection world with the main world, this means that every object in the main world will be added to the prospection world and vice versa. """ if self.is_prospection_world: # then no need to add another prospection world @@ -297,7 +296,7 @@ def _sync_prospection_world(self): def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, description: ObjectDescription, name: str) -> str: """ - Updates the cache directory with the given object. + Update the cache directory with the given object. :param path: The path to the object. :param ignore_cached_files: If the cached files should be ignored. @@ -317,7 +316,7 @@ def simulation_time_step(self): def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, obj_type: Optional[ObjectType] = None) -> int: """ - Loads a description file (e.g. URDF) at the given pose and returns the id of the loaded object. + Load a description file (e.g. URDF) at the given pose and returns the id of the loaded object. :param path: The path to the description file, if None the description file is assumed to be already loaded. :param pose: The pose at which the object should be loaded. @@ -328,13 +327,13 @@ def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose def load_generic_object_and_get_id(self, description: GenericObjectDescription) -> int: """ - Creates a visual and collision box in the simulation and returns the id of the loaded object. + Create a visual and collision box in the simulation and returns the id of the loaded object. """ raise NotImplementedError def get_object_names(self) -> List[str]: """ - Returns the names of all objects in the World. + Return the names of all objects in the World. :return: A list of object names. """ @@ -353,7 +352,7 @@ def get_object_by_name(self, name: str) -> Optional[Object]: def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: """ - Returns a list of all Objects which have the type 'obj_type'. + Return a list of all Objects which have the type 'obj_type'. :param obj_type: The type of the returned Objects. :return: A list of all Objects that have the type 'obj_type'. @@ -362,33 +361,34 @@ def get_object_by_type(self, obj_type: ObjectType) -> List[Object]: def get_object_by_id(self, obj_id: int) -> Object: """ - Returns the single Object that has the unique id. + Return the single Object that has the unique id. :param obj_id: The unique id for which the Object should be returned. :return: The Object with the id 'id'. """ return list(filter(lambda obj: obj.id == obj_id, self.objects))[0] - def remove_object_by_id(self, obj_id: int) -> bool: + def remove_visual_object(self, obj_id: int) -> bool: """ - Removes the object with the given id from the world, and saves a new original state for the world. + Remove the object with the given id from the world, and saves a new original state for the world. :param obj_id: The unique id of the object to be removed. :return: Whether the object was removed successfully. """ - removed = self._remove_object_by_id(obj_id) + + removed = self._remove_visual_object(obj_id) if removed: - self.original_state_id = self.save_state() + self.update_simulator_state_id_in_original_state() else: rospy.logwarn(f"Object with id {obj_id} could not be removed.") return removed @abstractmethod - def _remove_object_by_id(self, obj_id: int) -> bool: + def _remove_visual_object(self, obj_id: int) -> bool: """ - Removes the object with the given id from the world. + Remove the visual object with the given id from the world, and update the simulator state in the original state. - :param obj_id: The unique id of the object to be removed. + :param obj_id: The unique id of the visual object to be removed. :return: Whether the object was removed successfully. """ pass @@ -396,7 +396,7 @@ def _remove_object_by_id(self, obj_id: int) -> bool: @abstractmethod def remove_object_from_simulator(self, obj: Object) -> bool: """ - Removes an object from the physics simulator. + Remove an object from the physics simulator. :param obj: The object to be removed. :return: Whether the object was removed successfully. @@ -405,7 +405,7 @@ def remove_object_from_simulator(self, obj: Object) -> bool: def remove_object(self, obj: Object) -> None: """ - Removes this object from the current world. + Remove this object from the current world. For the object to be removed it has to be detached from all objects it is currently attached to. After this is done a call to world remove object is done to remove this Object from the simulation/world. @@ -418,18 +418,33 @@ def remove_object(self, obj: Object) -> None: if self.remove_object_from_simulator(obj): self.objects.remove(obj) - latest_state: WorldState = self._saved_states[self.original_state_id] - latest_state.object_states.pop(obj.name) + self.remove_object_from_original_state(obj) if World.robot == obj: World.robot = None self.object_lock.release() + def remove_object_from_original_state(self, obj: Object) -> None: + """ + Remove an object from the original state of the world. + + :param obj: The object to be removed. + """ + self.original_state.object_states.pop(obj.name) + + def add_object_to_original_state(self, obj: Object) -> None: + """ + Add an object to the original state of the world. + + :param obj: The object to be added. + """ + self.original_state.object_states[obj.name] = obj.current_state + def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: """ - Creates a fixed joint constraint between the given parent and child links, + Create a fixed joint constraint between the given parent and child links, the joint frame will be at the origin of the child link frame, and would have the same orientation as the child link frame. @@ -480,7 +495,7 @@ def get_joint_position(self, joint: Joint) -> float: @abstractmethod def get_object_joint_names(self, obj: Object) -> List[str]: """ - Returns the names of all joints of this object. + Return the names of all joints of this object. :param obj: The object. :return: A list of joint names. @@ -550,7 +565,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @abstractmethod def get_object_link_names(self, obj: Object) -> List[str]: """ - Returns the names of all links of this object. + Return the names of all links of this object. :param obj: The object. :return: A list of link names. @@ -559,7 +574,7 @@ def get_object_link_names(self, obj: Object) -> List[str]: def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: """ - Simulates Physics in the World for a given amount of seconds. Usually this simulation is faster than real + Simulate Physics in the World for a given amount of seconds. Usually this simulation is faster than real time. By setting the 'real_time' parameter this simulation is slowed down such that the simulated time is equal to real time. @@ -584,7 +599,7 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: def update_all_objects_poses(self) -> None: """ - Updates the positions of all objects in the world. + Update the positions of all objects in the world. """ for obj in self.objects: obj.update_pose() @@ -634,7 +649,7 @@ def get_object_orientation(self, obj: Object) -> List[float]: @property def robot_virtual_joints(self) -> List[Joint]: """ - Returns the virtual joints of the robot. + Return the virtual joints of the robot. """ return [self.robot.joints[name] for name in self.robot_virtual_joints_names] @@ -673,7 +688,7 @@ def get_object_contact_points(self, obj: Object) -> ContactPointsList: @abstractmethod def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList: """ - Returns a list of contact points between obj_a and obj_b. + Return a list of contact points between obj_a and obj_b. :param obj1: The first object. :param obj2: The second object. @@ -683,7 +698,7 @@ def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> def get_object_closest_points(self, obj: Object, max_distance: float) -> ClosestPointsList: """ - Returns the closest points of this object with all other objects in the world. + Return the closest points of this object with all other objects in the world. :param obj: The object. :param max_distance: The maximum distance between the points. @@ -697,7 +712,7 @@ def get_object_closest_points(self, obj: Object, max_distance: float) -> Closest def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \ -> ClosestPointsList: """ - Returns the closest points between two objects. + Return the closest points between two objects. :param object_a: The first object. :param object_b: The second object. @@ -784,7 +799,7 @@ def get_arm_tool_frame_link(self, arm: Arms) -> Link: @abstractmethod def set_link_color(self, link: Link, rgba_color: Color): """ - Changes the rgba_color of a link of this object, the rgba_color has to be given as Color object. + Change the rgba_color of a link of this object, the rgba_color has to be given as Color object. :param link: The link which should be colored. :param rgba_color: The rgba_color as Color object with RGBA values between 0 and 1. @@ -814,7 +829,7 @@ def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: @abstractmethod def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of this object. The return of this method are two points in + Return the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. :param obj: The object for which the bounding box should be returned. @@ -825,7 +840,7 @@ def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundi @abstractmethod def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box of the link. The return of this method are two points in + Return the axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ pass @@ -833,7 +848,7 @@ def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingB @abstractmethod def set_realtime(self, real_time: bool) -> None: """ - Enables the real time simulation of Physics in the World. By default, this is disabled and Physics is only + Enable the real time simulation of Physics in the World. By default, this is disabled and Physics is only simulated to reason about it. :param real_time: Whether the World should simulate Physics in real time. @@ -843,7 +858,7 @@ def set_realtime(self, real_time: bool) -> None: @abstractmethod def set_gravity(self, gravity_vector: List[float]) -> None: """ - Sets the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). + Set the gravity that is used in the World. By default, it is set to the gravity on earth ([0, 0, -9.8]). Gravity is given as a vector in x,y,z. Gravity is only applied while simulating Physic. :param gravity_vector: The gravity vector that should be used in the World. @@ -852,7 +867,7 @@ def set_gravity(self, gravity_vector: List[float]) -> None: def set_robot_if_not_set(self, robot: Object) -> None: """ - Sets the robot if it is not set yet. + Set the robot if it is not set yet. :param robot: The Object reference to the Object representing the robot. """ @@ -862,7 +877,7 @@ def set_robot_if_not_set(self, robot: Object) -> None: @staticmethod def set_robot(robot: Union[Object, None]) -> None: """ - Sets the global variable for the robot Object This should be set on spawning the robot. + Set the global variable for the robot Object This should be set on spawning the robot. :param robot: The Object reference to the Object representing the robot. """ @@ -871,7 +886,7 @@ def set_robot(robot: Union[Object, None]) -> None: @staticmethod def robot_is_set() -> bool: """ - Returns whether the robot has been set or not. + Return whether the robot has been set or not. :return: True if the robot has been set, False otherwise. """ @@ -879,7 +894,7 @@ def robot_is_set() -> bool: def exit(self, remove_saved_states: bool = True) -> None: """ - Closes the World as well as the prospection world, also collects any other thread that is running. + Close the World as well as the prospection world, also collects any other thread that is running. :param remove_saved_states: Whether to remove the saved states. """ @@ -894,7 +909,7 @@ def exit(self, remove_saved_states: bool = True) -> None: def exit_prospection_world_if_exists(self) -> None: """ - Exits the prospection world if it exists. + Exit the prospection world if it exists. """ if self.prospection_world: self.terminate_world_sync() @@ -903,13 +918,13 @@ def exit_prospection_world_if_exists(self) -> None: @abstractmethod def disconnect_from_physics_server(self) -> None: """ - Disconnects the world from the physics server. + Disconnect the world from the physics server. """ pass def reset_current_world(self) -> None: """ - Resets the pose of every object in the World to the pose it was spawned in and sets every joint to 0. + Reset the pose of every object in the World to the pose it was spawned in and sets every joint to 0. """ for obj in self.objects: obj.set_pose(obj.original_pose) @@ -917,7 +932,7 @@ def reset_current_world(self) -> None: def reset_robot(self) -> None: """ - Sets the robot class variable to None. + Set the robot class variable to None. """ self.set_robot(None) @@ -930,7 +945,7 @@ def join_threads(self) -> None: def terminate_world_sync(self) -> None: """ - Terminates the world sync thread. + Terminate the world sync thread. """ self.world_sync.terminate = True self.resume_world_sync() @@ -938,7 +953,7 @@ def terminate_world_sync(self) -> None: def save_state(self, state_id: Optional[int] = None) -> int: """ - Returns the id of the saved state of the World. The saved state contains the states of all the objects and + 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. :return: A unique id of the state @@ -963,7 +978,7 @@ def current_state(self, state: WorldState) -> None: @property def object_states(self) -> Dict[str, ObjectState]: """ - Returns the states of all objects in the World. + Return the states of all objects in the World. :return: A dictionary with the object id as key and the object state as value. """ @@ -972,14 +987,14 @@ def object_states(self) -> Dict[str, ObjectState]: @object_states.setter def object_states(self, states: Dict[str, ObjectState]) -> None: """ - Sets the states of all objects in the World. + Set the states of all objects in the World. """ for obj_name, obj_state in states.items(): self.get_object_by_name(obj_name).current_state = obj_state def save_objects_state(self, state_id: int) -> None: """ - Saves the state of all objects in the World according to the given state using the unique state id. + Save the state of all objects in the World according to the given state using the unique state id. :param state_id: The unique id representing the state. """ @@ -989,7 +1004,7 @@ def save_objects_state(self, state_id: int) -> None: @abstractmethod def save_physics_simulator_state(self) -> int: """ - Saves the state of the physics simulator and returns the unique id of the state. + Save the state of the physics simulator and returns the unique id of the state. :return: The unique id representing the state. """ @@ -998,7 +1013,7 @@ def save_physics_simulator_state(self) -> int: @abstractmethod def remove_physics_simulator_state(self, state_id: int) -> None: """ - Removes the state of the physics simulator with the given id. + Remove the state of the physics simulator with the given id. :param state_id: The unique id representing the state. """ @@ -1007,7 +1022,7 @@ def remove_physics_simulator_state(self, state_id: int) -> None: @abstractmethod def restore_physics_simulator_state(self, state_id: int) -> None: """ - Restores the objects and environment state in the physics simulator according to + Restore the objects and environment state in the physics simulator according to the given state using the unique state id. :param state_id: The unique id representing the state. @@ -1019,7 +1034,7 @@ def get_images_for_target(self, cam_pose: Pose, size: Optional[int] = 256) -> List[np.ndarray]: """ - Calculates the view and projection Matrix and returns 3 images: + Calculate the view and projection Matrix and returns 3 images: 1. An RGB image 2. A depth image @@ -1038,7 +1053,7 @@ def register_two_objects_collision_callbacks(self, on_collision_callback: Callable, on_collision_removal_callback: Optional[Callable] = None) -> None: """ - Registers callback methods for contact between two Objects. There can be a callback for when the two Objects + Register callback methods for contact between two Objects. There can be a callback for when the two Objects get in contact and, optionally, for when they are not in contact anymore. :param object_a: An object in the World @@ -1059,7 +1074,7 @@ def data_directories(self): @classmethod def add_resource_path(cls, path: str, prepend: bool = False) -> None: """ - Adds a resource path in which the World will search for files. This resource directory is searched if an + Add a resource path in which the World will search for files. This resource directory is searched if an Object is spawned only with a filename. :param path: A path in the filesystem in which to search for files. @@ -1090,7 +1105,7 @@ def change_cache_dir_path(cls, path: str) -> None: def get_prospection_object_for_object(self, obj: Object) -> Object: """ - Returns the corresponding object from the prospection world for a given object in the main world. + Return the corresponding object from the prospection world for a given object in the main world. If the given Object is already in the prospection world, it is returned. :param obj: The object for which the corresponding object in the prospection World should be found. @@ -1101,7 +1116,7 @@ def get_prospection_object_for_object(self, obj: Object) -> Object: def get_object_for_prospection_object(self, prospection_object: Object) -> Object: """ - Returns the corresponding object from the main World for a given + Return the corresponding object from the main World for a given object in the prospection world. If the given object is not in the prospection world an error will be raised. @@ -1112,7 +1127,7 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec def remove_all_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: """ - Removes all objects from the World. + Remove all objects from the World. :param exclude_objects: A list of objects that should not be removed. """ @@ -1122,7 +1137,7 @@ def remove_all_objects(self, exclude_objects: Optional[List[Object]] = None) -> def reset_world(self, remove_saved_states=False) -> None: """ - Resets the World to the state it was first spawned in. + Reset the World to the state it was first spawned in. All attached objects will be detached, all joints will be set to the default position of 0 and all objects will be set to the position and orientation in which they were spawned. @@ -1136,14 +1151,20 @@ def reset_world(self, remove_saved_states=False) -> None: def remove_saved_states(self) -> None: """ - Removes all saved states of the World. + Remove all saved states of the World. """ for state_id in self.saved_states: self.remove_physics_simulator_state(state_id) + self.remove_objects_saved_states() super().remove_saved_states() + self.original_state_id = None + + def remove_objects_saved_states(self) -> None: + """ + Remove all saved states of the objects in the World. + """ for obj in self.objects: obj.remove_saved_states() - self.original_state_id = None def update_transforms_for_objects_in_current_world(self) -> None: """ @@ -1458,7 +1479,7 @@ def _remove_vis_axis(self) -> None: def _simulator_object_creator(self, creator_func: Callable, *args, **kwargs) -> int: """ - Creates an object in the physics simulator and returns the created object id. + Create an object in the physics simulator and returns the created object id. :param creator_func: The function that creates the object in the physics simulator. :param args: The arguments for the creator function. @@ -1466,21 +1487,32 @@ def _simulator_object_creator(self, creator_func: Callable, *args, **kwargs) -> :return: The created object id. """ obj_id = creator_func(*args, **kwargs) - latest_state: WorldState = self._saved_states[self.original_state_id] - latest_state.simulator_state_id = self.save_physics_simulator_state() + self.update_simulator_state_id_in_original_state() return obj_id def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) -> None: """ - Removes an object from the physics simulator. + Remove an object from the physics simulator. :param remover_func: The function that removes the object from the physics simulator. :param args: The arguments for the remover function. :param kwargs: The keyword arguments for the remover function. """ remover_func(*args, **kwargs) - latest_state: WorldState = self._saved_states[self.original_state_id] - latest_state.simulator_state_id = self.save_physics_simulator_state() + self.update_simulator_state_id_in_original_state() + + def update_simulator_state_id_in_original_state(self) -> None: + """ + Update the simulator state id in the original state. + """ + self.original_state.simulator_state_id = self.save_physics_simulator_state() + + @property + def original_state(self) -> WorldState: + """ + The saved original state of the world. + """ + return self.saved_states[self.original_state_id] def __del__(self): self.exit() diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 385d6b9c5..0fb7341cc 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -9,7 +9,7 @@ import rosgraph import rospy from geometry_msgs.msg import Point -from typing_extensions import List, Optional, Dict +from typing_extensions import List, Optional, Dict, Any from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ ClosestPoint, LateralFriction, ContactPoint, ContactPointsList, ClosestPointsList @@ -102,13 +102,22 @@ def _load_object_and_get_id(self, path: str, pose: Pose) -> int: basePosition=pose.position_as_list(), baseOrientation=pose.orientation_as_list(), physicsClientId=self.id) - def remove_object_from_simulator(self, obj: Object) -> bool: - return self._remove_object_by_id(obj.id) + def _remove_visual_object(self, obj_id: int) -> bool: + self._remove_body(obj_id) + return True - def _remove_object_by_id(self, obj_id: int) -> True: - p.removeBody(obj_id, self.id) + def remove_object_from_simulator(self, obj: Object) -> bool: + self._remove_body(obj.id) return True + def _remove_body(self, body_id: int) -> Any: + """ + Remove a body from PyBullet using the body id. + + :param body_id: The id of the body. + """ + return p.removeBody(body_id, self.id) + def add_constraint(self, constraint: Constraint) -> int: constraint_id = p.createConstraint(constraint.parent_object_id, diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index c1687741c..cb073e0a8 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -485,9 +485,9 @@ def join_threads(self) -> None: self.reader.stop_thread = True self.reader.join() - def _remove_object_by_id(self, obj_id: int) -> bool: - obj = self.get_object_by_id(obj_id) - return self.remove_object_from_simulator(obj) + def _remove_visual_object(self, obj_id: int) -> bool: + rospy.logwarn("Currently multiverse does not create visual objects") + return False def remove_object_from_simulator(self, obj: Object) -> bool: if obj.obj_type != ObjectType.ENVIRONMENT: From bb95b6dd19d09a1f5c9ba3ea7824903e813110f9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 23 Aug 2024 16:54:50 +0200 Subject: [PATCH 164/189] [MultiverseMJCF] Added load and save in multiverse. Added a configuration variable to know whether to use simulator save and restore state. --- src/pycram/config/multiverse_conf.py | 5 +++ src/pycram/config/world_conf.py | 7 ++++ src/pycram/datastructures/dataclasses.py | 2 +- src/pycram/datastructures/world.py | 36 ++++++++++++------ src/pycram/worlds/bullet_world.py | 2 +- src/pycram/worlds/multiverse.py | 31 ++++++++------- .../multiverse_communication/clients.py | 38 +++++++++++++++++-- .../worlds/multiverse_datastructures/enums.py | 2 + 8 files changed, 93 insertions(+), 30 deletions(-) diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 56d9b6cf7..19ba149bb 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -53,6 +53,11 @@ The default description type for the objects. """ +use_physics_simulator_state: bool = True +""" +Whether to use the physics simulator state when restoring or saving the world state. +""" + job_handling: world_conf.JobHandling = world_conf.JobHandling(let_pycram_move_attached_objects=False, let_pycram_handle_spawning=False) diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index 045b7c5f5..93f101644 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -42,6 +42,13 @@ The default description type for the objects. """ +use_physics_simulator_state: bool = False +""" +Whether to use the physics simulator state when restoring or saving the world state. +Currently with PyBullet, this causes a bug where ray_test does not work correctly after restoring the state using the +simulator, so it is recommended to set this to False in PyBullet. +""" + @dataclass class JobHandling: diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 73ef87c9b..56b2c3fa8 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -410,7 +410,7 @@ class WorldState(State): """ Dataclass for storing the state of the world. """ - simulator_state_id: int + simulator_state_id: Optional[int] object_states: Dict[str, ObjectState] def __eq__(self, other: 'WorldState'): diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 188878197..21ab87b82 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -432,6 +432,7 @@ def remove_object_from_original_state(self, obj: Object) -> None: :param obj: The object to be removed. """ self.original_state.object_states.pop(obj.name) + self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=True) def add_object_to_original_state(self, obj: Object) -> None: """ @@ -440,6 +441,7 @@ def add_object_to_original_state(self, obj: Object) -> None: :param obj: The object to be added. """ self.original_state.object_states[obj.name] = obj.current_state + self.update_simulator_state_id_in_original_state() def add_fixed_constraint(self, parent_link: Link, child_link: Link, child_to_parent_transform: Transform) -> int: @@ -966,14 +968,18 @@ def save_state(self, state_id: Optional[int] = None) -> int: @property def current_state(self) -> WorldState: if self._current_state is None: - self._current_state = WorldState(self.save_physics_simulator_state(), self.object_states) + simulator_state = None if conf.use_physics_simulator_state else self.save_physics_simulator_state(True) + self._current_state = WorldState(simulator_state, self.object_states) return WorldState(self._current_state.simulator_state_id, self.object_states) @current_state.setter def current_state(self, state: WorldState) -> None: if self.current_state != state: - for obj in self.objects: - self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] + if conf.use_physics_simulator_state: + self.restore_physics_simulator_state(state.simulator_state_id) + else: + for obj in self.objects: + self.get_object_by_name(obj.name).current_state = state.object_states[obj.name] @property def object_states(self) -> Dict[str, ObjectState]: @@ -1002,10 +1008,11 @@ def save_objects_state(self, state_id: int) -> None: obj.save_state(state_id) @abstractmethod - def save_physics_simulator_state(self) -> int: + def save_physics_simulator_state(self, 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. :return: The unique id representing the state. """ pass @@ -1153,9 +1160,11 @@ def remove_saved_states(self) -> None: """ Remove all saved states of the World. """ - for state_id in self.saved_states: - self.remove_physics_simulator_state(state_id) - self.remove_objects_saved_states() + if conf.use_physics_simulator_state: + for state_id in self.saved_states: + self.remove_physics_simulator_state(state_id) + else: + self.remove_objects_saved_states() super().remove_saved_states() self.original_state_id = None @@ -1463,7 +1472,7 @@ def _add_vis_axis(self, pose: Pose) -> None: """ See :py:meth:`~pycram.world.World.add_vis_axis` """ - raise NotImplementedError + rospy.logwarn(f"Visual axis is not supported in {self.__class__.__name__}") def remove_vis_axis(self) -> None: """ @@ -1475,7 +1484,7 @@ def _remove_vis_axis(self) -> None: """ See :py:meth:`~pycram.world.World.remove_vis_axis` """ - raise NotImplementedError + rospy.logwarn(f"Visual axis is not supported in {self.__class__.__name__}") def _simulator_object_creator(self, creator_func: Callable, *args, **kwargs) -> int: """ @@ -1501,11 +1510,14 @@ def _simulator_object_remover(self, remover_func: Callable, *args, **kwargs) -> remover_func(*args, **kwargs) self.update_simulator_state_id_in_original_state() - def update_simulator_state_id_in_original_state(self) -> None: + def update_simulator_state_id_in_original_state(self, use_same_id: bool = False) -> None: """ - Update the simulator state id in the original state. + Update the simulator state id in the original state if use_physics_simulator_state is True in the configuration. + + :param use_same_id: If the same id should be used for the state. """ - self.original_state.simulator_state_id = self.save_physics_simulator_state() + if conf.use_physics_simulator_state: + self.original_state.simulator_state_id = self.save_physics_simulator_state(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 0fb7341cc..e6985e60a 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -298,7 +298,7 @@ def join_gui_thread_if_exists(self): if self._gui_thread: self._gui_thread.join() - def save_physics_simulator_state(self) -> int: + def save_physics_simulator_state(self, 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 cb073e0a8..a5c70897b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -81,6 +81,8 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, :param simulation: The name of the simulation. """ + self.latest_save_id: Optional[int] = None + self.saved_simulator_states: Dict = {} self._make_sure_multiverse_resources_are_added() if Multiverse.simulation is None: @@ -90,13 +92,12 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, Multiverse.simulation = simulation self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + self.client_manager = MultiverseClientManager(self.simulation_wait_time_factor) + self._init_clients(is_prospection=is_prospection) World.__init__(self, mode, is_prospection, simulation_frequency, **conf.job_handling.as_dict(), **conf.error_tolerance.as_dict()) - self.client_manager = MultiverseClientManager(self.simulation_wait_time_factor) - self._init_clients() - self._init_constraint_and_object_id_name_map_collections() self.ray_test_utils = RayTestUtils(self.ray_test_batch, self.object_id_to_name) @@ -107,23 +108,25 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if self.use_bullet_mode: self.api_requester.pause_simulation() - def _init_clients(self): + def _init_clients(self, is_prospection: bool = False): """ Initialize the Multiverse clients that will be used to communicate with the Multiverse server. Each client is responsible for a specific task, e.g. reading data from the server, writing data to the serve, calling the API, or controlling the robot joints. + + :param is_prospection: Whether the world is prospection or not. """ self.reader: MultiverseReader = self.client_manager.create_reader( - is_prospection_world=self.is_prospection_world) + is_prospection_world=is_prospection) self.writer: MultiverseWriter = self.client_manager.create_writer( self.simulation, - is_prospection_world=self.is_prospection_world) + is_prospection_world=is_prospection) self.api_requester: MultiverseAPI = self.client_manager.create_api_requester( self.simulation, - is_prospection_world=self.is_prospection_world) + is_prospection_world=is_prospection) if self.use_controller: self.joint_controller: MultiverseController = self.client_manager.create_controller( - is_prospection_world=self.is_prospection_world) + is_prospection_world=is_prospection) def _init_constraint_and_object_id_name_map_collections(self): self.last_object_id: int = -1 @@ -621,15 +624,17 @@ def step(self): sleep(self.simulation_time_step) self.api_requester.pause_simulation() - def save_physics_simulator_state(self) -> int: - logging.warning("save_physics_simulator_state is not implemented in Multiverse") - return 0 + 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 remove_physics_simulator_state(self, state_id: int) -> None: - logging.warning("remove_physics_simulator_state is not implemented in Multiverse") + self.saved_simulator_states.pop(state_id) def restore_physics_simulator_state(self, state_id: int) -> None: - logging.warning("restore_physics_simulator_state is not implemented in Multiverse") + 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") diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 3313b74ac..754d7f987 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -1,5 +1,6 @@ import datetime import logging +import os import threading from time import time, sleep @@ -10,11 +11,11 @@ from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) +from ...config import multiverse_conf as conf from ...datastructures.pose import Pose from ...datastructures.world import World from ...world_concepts.constraints import Constraint from ...world_concepts.world_object import Object, Link -from ...config import multiverse_conf as conf class MultiverseClient(MultiverseSocket): @@ -40,7 +41,6 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False, class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = conf.READER_MAX_WAIT_TIME_FOR_DATA """ The maximum wait time for the data in seconds. @@ -512,7 +512,6 @@ def init_controller(self, actuator_joint_commands: Dict[str, List[str]]) -> None class MultiverseAPI(MultiverseClient): - API_REQUEST_WAIT_TIME: datetime.timedelta = datetime.timedelta(milliseconds=200) """ The wait time for the API request in seconds. @@ -536,6 +535,39 @@ def __init__(self, name: str, port: int, simulation: str, is_prospection_world: self.simulation = simulation self.wait: bool = False # Whether to wait after sending the API request. + def save(self, save_name: str, save_directory: Optional[str] = None) -> str: + """ + Save the current state of the simulation. + + :param save_name: The name of the save. + :param save_directory: The path to save the simulation, can be relative or absolute. If the path is relative, + it will be saved in the saved folder in multiverse. + :return: The save path. + """ + response = self._request_single_api_callback(API.SAVE, self.get_save_path(save_name, save_directory)) + return response[0] + + def load(self, save_name: str, save_directory: Optional[str] = None) -> None: + """ + Load the saved state of the simulation. + + :param save_name: The name of the save. + :param save_directory: The path to load the simulation, can be relative or absolute. If the path is relative, + it will be loaded from the saved folder in multiverse. + """ + self._request_single_api_callback(API.LOAD, self.get_save_path(save_name, save_directory)) + + @staticmethod + def get_save_path(save_name: str, save_directory: Optional[str] = None) -> str: + """ + Get the save path. + + :param save_name: The save name. + :param save_directory: The save directory. + :return: The save path. + """ + return save_name if save_directory is None else os.path.join(save_directory, save_name) + def attach(self, constraint: Constraint) -> None: """ Request to attach the child link to the parent link. diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py index d6028a46a..b30647954 100644 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ b/src/pycram/worlds/multiverse_datastructures/enums.py @@ -16,6 +16,8 @@ class MultiverseAPIName(Enum): EXIST = "exist" PAUSE = "pause" UNPAUSE = "unpause" + SAVE = "save" + LOAD = "load" class MultiverseProperty(Enum): From 75f10fc2a8b43741108243617d57285eecb3c891 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 23 Aug 2024 20:28:27 +0200 Subject: [PATCH 165/189] [MultiverseMJCF] Pycram can now handle .ply mesh files as well. Also supports scaling the mesh by using scale_mesh argument when initializing the object. A new requirement for loading .ply meshes and for scaling meshes is added (trimesh). --- requirements.txt | 3 ++- src/pycram/cache_manager.py | 9 ++++++--- src/pycram/datastructures/world.py | 7 +++++-- src/pycram/description.py | 13 +++++++++++-- src/pycram/world_concepts/world_object.py | 7 +++++-- src/pycram/worlds/multiverse.py | 8 ++++---- test/test_multiverse.py | 2 +- 7 files changed, 34 insertions(+), 15 deletions(-) diff --git a/requirements.txt b/requirements.txt index ae111afde..23355f6f1 100644 --- a/requirements.txt +++ b/requirements.txt @@ -24,4 +24,5 @@ pynput~=1.7.7 playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 -dm_control \ No newline at end of file +dm_control +trimesh \ No newline at end of file diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 0be43296f..cbd3cac31 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -3,7 +3,7 @@ import pathlib import shutil -from typing_extensions import List, TYPE_CHECKING +from typing_extensions import List, TYPE_CHECKING, Optional if TYPE_CHECKING: from .description import ObjectDescription @@ -41,7 +41,8 @@ def delete_cache_dir(self): shutil.rmtree(self.cache_dir) def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - object_description: 'ObjectDescription', object_name: str) -> str: + object_description: 'ObjectDescription', object_name: str, + scale_mesh: Optional[float] = None) -> str: """ Check if the file is already in the cache directory, if not preprocess and save in the cache. @@ -50,6 +51,8 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, is already cached. :param object_description: The object description of the file. :param object_name: The name of the object. + :param scale_mesh: The scale of the mesh. + :return: The path of the cached file. """ path_object = pathlib.Path(path) extension = path_object.suffix @@ -62,7 +65,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, if not self.is_cached(path, object_description) or ignore_cached_files: # if file is not yet cached preprocess the description file and save it in the cache directory. path = self.look_for_file_in_data_dir(path_object) - object_description.generate_description_from_file(path, object_name, extension, cache_path) + object_description.generate_description_from_file(path, object_name, extension, cache_path, scale_mesh) return cache_path diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 21ab87b82..41fd96307 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -294,7 +294,8 @@ def _sync_prospection_world(self): self.world_sync.start() def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, - description: ObjectDescription, name: str) -> str: + description: ObjectDescription, name: str, + scale_mesh: Optional[float] = None) -> str: """ Update the cache directory with the given object. @@ -302,8 +303,10 @@ def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached :param ignore_cached_files: If the cached files should be ignored. :param description: The object description. :param name: The name of the object. + :param scale_mesh: The scale of the mesh. + :return: The path of the cached object. """ - return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name) + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name, scale_mesh) @property def simulation_time_step(self): diff --git a/src/pycram/description.py b/src/pycram/description.py index f99cb58e4..f05b14248 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -6,6 +6,7 @@ from abc import ABC, abstractmethod import rospy +import trimesh from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING @@ -619,7 +620,7 @@ class ObjectDescription(EntityDescription): A class that represents the description of an object. """ - mesh_extensions: Tuple[str] = (".obj", ".stl", ".dae") + mesh_extensions: Tuple[str] = (".obj", ".stl", ".dae", ".ply") """ The file extensions of the mesh files that can be used to generate a description file. """ @@ -760,7 +761,8 @@ def load_description(self, path: str) -> Any: """ pass - def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str) -> None: + def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str, + scale_mesh: Optional[float] = None) -> None: """ Generate and preprocess the description from the file at the given path and save the preprocessed description. The generated description will be saved at the given save path. @@ -769,10 +771,17 @@ def generate_description_from_file(self, path: str, name: str, extension: str, s :param name: The name of the object. :param extension: The file extension of the file to preprocess. :param save_path: The path to save the generated description file. + :param scale_mesh: The scale of the mesh. :raises ObjectDescriptionNotFound: If the description file could not be found/read. """ if extension in self.mesh_extensions: + if extension == ".ply": + mesh = trimesh.load(path) + path = path.replace(extension, ".stl") + if scale_mesh is not None: + mesh.apply_scale(scale_mesh) + mesh.export(path) self.generate_from_mesh_file(path, name, save_path=save_path) elif extension == self.get_file_extension(): self.generate_from_description_file(path, save_path=save_path) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index ffb64193f..f58920fbf 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -50,7 +50,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, pose: Optional[Pose] = None, world: Optional[World] = None, color: Color = Color(), - ignore_cached_files: bool = False): + ignore_cached_files: bool = False, + scale_mesh: Optional[float] = None): """ The constructor loads the description file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -67,6 +68,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :py:attr:`~World.current_world` will be used. :param color: The rgba_color with which the object should be spawned. :param ignore_cached_files: If true the file will be spawned while ignoring cached files. + :param scale_mesh: The scale of the mesh. """ super().__init__(-1, world if world is not None else World.current_world) @@ -86,7 +88,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, if path is not None: self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, - self.description, self.name) + self.description, self.name, + scale_mesh=scale_mesh) self.description.update_description_from_file(self.path) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index a5c70897b..6c5b0128b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -71,14 +71,14 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, - simulation: Optional[str] = None): + simulation_name: str = "pycram_test"): """ Initialize the Multiverse Socket and the PyCram World. :param mode: The mode of the world (DIRECT or GUI). :param is_prospection: Whether the world is prospection or not. :param simulation_frequency: The frequency of the simulation. - :param simulation: The name of the simulation. + :param simulation_name: The name of the simulation. """ self.latest_save_id: Optional[int] = None @@ -86,10 +86,10 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self._make_sure_multiverse_resources_are_added() if Multiverse.simulation is None: - if simulation is None: + if simulation_name is None: logging.error("Simulation name not provided") raise ValueError("Simulation name not provided") - Multiverse.simulation = simulation + Multiverse.simulation = simulation_name self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation self.client_manager = MultiverseClientManager(self.simulation_wait_time_factor) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4b36f8f6a..ac152aa80 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -44,7 +44,7 @@ class MultiversePyCRAMTestCase(unittest.TestCase): def setUpClass(cls): if not multiverse_installed: return - cls.multiverse = Multiverse(simulation="pycram_test") + cls.multiverse = Multiverse() @classmethod def tearDownClass(cls): From 056fddcb852aac25983f2084dc045e0b9e996772 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Sun, 25 Aug 2024 13:44:24 +0200 Subject: [PATCH 166/189] [MultiverseMJCF] Tested without having a multiverse installation. --- src/pycram/datastructures/world.py | 13 ++++++++++--- src/pycram/object_descriptors/mjcf.py | 17 +++++++++++------ .../robot_descriptions/pr2_description.py | 2 +- src/pycram/world_concepts/world_object.py | 8 +++++--- src/pycram/worlds/multiverse.py | 6 ++++++ .../worlds/multiverse_extras/helpers.py | 19 +++++++++++++++---- test/test_bullet_world.py | 2 +- test/test_mjcf.py | 8 ++++++-- 8 files changed, 55 insertions(+), 20 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 41fd96307..6eec27f10 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -165,6 +165,13 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.original_state_id = self.save_state() + @classmethod + def get_cache_dir(cls) -> str: + """ + Return the cache directory. + """ + return cls.cache_manager.cache_dir + def add_object(self, obj: Object) -> None: """ Add an object to the world. @@ -1074,12 +1081,12 @@ def register_two_objects_collision_callbacks(self, self.coll_callbacks[(object_a, object_b)] = CollisionCallbacks(on_collision_callback, on_collision_removal_callback) - @property - def data_directories(self): + @classmethod + def get_data_directories(cls) -> List[str]: """ The resources directories where the objects, robots, and environments are stored. """ - return self.cache_manager.data_directories + return cls.cache_manager.data_directories @classmethod def add_resource_path(cls, path: str, prepend: bool = False) -> None: diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 588bed48b..686abe1e6 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -14,12 +14,17 @@ SphereVisualShape, MeshVisualShape from ..failures import MultiplePossibleTipLinks -from multiverse_parser import Configuration, Factory, InertiaSource -from multiverse_parser import (WorldBuilder, - GeomType, GeomProperty, - MeshProperty) -from multiverse_parser import MjcfExporter -from pxr import Usd, UsdGeom +try: + from multiverse_parser import Configuration, Factory, InertiaSource + from multiverse_parser import (WorldBuilder, + GeomType, GeomProperty, + MeshProperty) + from multiverse_parser import MjcfExporter + from pxr import Usd, UsdGeom +except ImportError: + rospy.logwarn("Multiverse not found.") + # do not import this module if multiverse is not found + raise ImportError("Multiverse not found.") class LinkDescription(AbstractLinkDescription): diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index 3664cea49..fc0c68736 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -9,7 +9,7 @@ rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' -mjcf_filename = get_robot_mjcf_path("willow_garage", "pr2") +mjcf_filename = get_robot_mjcf_path("", "pr2") pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", filename, virtual_move_base_joints=VirtualMoveBaseJoints(), mjcf_path=mjcf_filename) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index f58920fbf..073ad8f17 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -22,7 +22,10 @@ ObjectDescriptionUndefined from ..local_transformer import LocalTransformer from ..object_descriptors.urdf import ObjectDescription as URDF -from ..object_descriptors.mjcf import ObjectDescription as MJCF +try: + from ..object_descriptors.mjcf import ObjectDescription as MJCF +except ImportError: + MJCF = None from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment @@ -39,8 +42,7 @@ class Object(WorldEntity): The prefix for the tf frame of objects in the prospection world. """ - extension_to_description_type: Dict[str, Type[ObjectDescription]] = {URDF.get_file_extension(): URDF, - MJCF.get_file_extension(): MJCF} + extension_to_description_type: Dict[str, Type[ObjectDescription]] = {URDF.get_file_extension(): URDF} """ A dictionary that maps the file extension to the corresponding ObjectDescription type. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6c5b0128b..8a53450bb 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -16,6 +16,7 @@ from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint, ObjectDescription +from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescription from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses @@ -68,6 +69,11 @@ class Multiverse(World): The default description type for the objects. """ + Object.extension_to_description_type[MJCF.get_file_extension()] = MJCF + """ + Add the MJCF description extension to the extension to description type mapping for the objects. + """ + def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py index 72a31a86f..43189402c 100644 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -1,5 +1,6 @@ import os +import rospy from typing_extensions import Optional, List, Dict import xml.etree.ElementTree as ET @@ -24,18 +25,28 @@ def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: return joint_actuators -def get_robot_mjcf_path(company_name: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: +def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: """ Get the path to the MJCF file of a robot. - :param company_name: The name of the company that created the robot. + :param robot_relative_dir: The relative directory of the robot in the Multiverse resources/robots directory. :param robot_name: The name of the robot. :param xml_name: The name of the XML file of the robot. - :return: The path to the MJCF file of the robot. + :return: The path to the MJCF file of the robot if it exists, otherwise None. """ xml_name = xml_name if xml_name is not None else robot_name + if '.xml' not in xml_name: + xml_name = xml_name + '.xml' multiverse_resources = find_multiverse_resources_path() + try: + robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name) + except TypeError: + rospy.logwarn("Multiverse resources path not found.") + return None if multiverse_resources is not None: - return os.path.join(multiverse_resources, 'robots', company_name, robot_name, 'mjcf', f'{robot_name}.xml') + if f'mjcf/{xml_name}' in os.listdir(robot_folder): + return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, 'mjcf', xml_name) + elif xml_name in os.listdir(robot_folder): + return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, xml_name) return None diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 8aa57fd1a..565e98342 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -140,7 +140,7 @@ def test_equal_world_states(self): def test_add_resource_path(self): self.world.add_resource_path("test") - self.assertTrue("test" in self.world.data_directories) + self.assertTrue("test" in self.world.get_data_directories()) def test_no_prospection_object_found_for_given_object(self): milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) diff --git a/test/test_mjcf.py b/test/test_mjcf.py index 8581866ce..bcb9e2262 100644 --- a/test/test_mjcf.py +++ b/test/test_mjcf.py @@ -1,8 +1,12 @@ -from unittest import TestCase +from unittest import TestCase, skipIf from dm_control import mjcf -from pycram.object_descriptors.mjcf import ObjectDescription as MJCFObjDesc +try: + from pycram.object_descriptors.mjcf import ObjectDescription as MJCFObjDesc +except ImportError: + MJCFObjDesc = None +@skipIf(MJCFObjDesc is None, "Multiverse not found.") class TestMjcf(TestCase): model: MJCFObjDesc From 1c19d11944273eaebacda9945a7a159e4e9e6794 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Tue, 27 Aug 2024 02:07:54 +0200 Subject: [PATCH 167/189] [MultiverseMJCF] Added get links contacts and check if two objects are in contact in the ContactPointsList. Removed unnecessary index and getitem methods. Added set color in links. --- src/pycram/datastructures/dataclasses.py | 36 +++++++++++++++++++----- src/pycram/description.py | 8 ++++++ 2 files changed, 37 insertions(+), 7 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 56b2c3fa8..0b2738bec 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -493,14 +493,35 @@ class ContactPointsList(list): """ A list of contact points. """ + def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: + """ + Return the links that are not in the current points list but were in the initial points list. + + :param previous_points: The initial points list. + :return: A list of Link instances that represent the links that got removed. + """ + initial_links_in_contact = previous_points.get_links_in_contact() + current_links_in_contact = self.get_links_in_contact() + return [link for link in initial_links_in_contact if link not in current_links_in_contact] + + def get_links_in_contact(self) -> List[Link]: + """ + Get the links in contact. - def __index__(self, index: int) -> ContactPoint: - return super().__getitem__(index) + :return: A list of Link instances that represent the links in contact. + """ + return [point.link_b for point in self] - def __getitem__(self, item) -> Union[ContactPoint, 'ContactPointsList']: - if isinstance(item, slice): - return ContactPointsList(super().__getitem__(item)) - return super().__getitem__(item) + def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> bool: + """ + Check if two objects are in contact. + + :param obj_a: An instance of the Object class that represents the first object. + :param obj_b: An instance of the Object class that represents the second object. + :return: True if the objects are in contact, False otherwise. + """ + return (any([point.link_b.object == obj_b and point.link_a.object == obj_a for point in self]) or + any([point.link_a.object == obj_b and point.link_b.object == obj_a for point in self])) def get_normals_of_object(self, obj: Object) -> List[List[float]]: """ @@ -604,8 +625,9 @@ class TextAnnotation: """ text: str position: List[float] - color: Color id: int + color: Color = Color(0, 0, 0, 1) + size: float = 0.1 @dataclass diff --git a/src/pycram/description.py b/src/pycram/description.py index f05b14248..0418bd82b 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -431,6 +431,14 @@ def color(self) -> Color: """ return self.world.get_link_color(self) + def set_color(self, color: Color) -> None: + """ + Set the color of this link, could be rgb or rgba. + + :param color: The color as a list of floats, either rgb or rgba. + """ + self.color = color + @color.setter def color(self, color: Color) -> None: """ From 3cbb2af8bf9b0c1598cd07f3b5131b81dd7a520a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 27 Aug 2024 16:32:22 +0200 Subject: [PATCH 168/189] [MultiverseMJCF] Use .obj for meshes instead of .stl as .obj contains the texture. return unique object names for contact points objects. --- src/pycram/datastructures/dataclasses.py | 16 ++++++++-------- src/pycram/description.py | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 0b2738bec..05fb2a2c9 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -589,21 +589,21 @@ def is_object_in_the_list(self, obj: Object) -> bool: """ return obj in self.get_objects_that_have_points() - def get_objects_that_have_points(self) -> List[Object]: + def get_names_of_objects_that_have_points(self) -> List[str]: """ - Return the objects that have points in the list. + Return the names of the objects that have points in the list. - :return: A list of Object instances that represent the objects that have points in the list. + :return: A list of strings that represent the names of the objects that have points in the list. """ - return [point.link_b.object for point in self] + return [obj.name for obj in self.get_objects_that_have_points()] - def get_names_of_objects_that_have_points(self) -> List[str]: + def get_objects_that_have_points(self) -> List[Object]: """ - Return the names of the objects that have points in the list. + Return the objects that have points in the list. - :return: A list of strings that represent the names of the objects that have points in the list. + :return: A list of Object instances that represent the objects that have points in the list. """ - return [point.link_b.object.name for point in self] + return list({point.link_b.object for point in self}) def __str__(self): return f"ContactPointsList: {', '.join(self.get_names_of_objects_that_have_points())}" diff --git a/src/pycram/description.py b/src/pycram/description.py index 0418bd82b..b5f1f3148 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -786,7 +786,7 @@ def generate_description_from_file(self, path: str, name: str, extension: str, s if extension in self.mesh_extensions: if extension == ".ply": mesh = trimesh.load(path) - path = path.replace(extension, ".stl") + path = path.replace(extension, ".obj") if scale_mesh is not None: mesh.apply_scale(scale_mesh) mesh.export(path) From 5793cf8c277d9f7940976dccb3196be97b1b9170 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 12 Sep 2024 13:50:15 +0200 Subject: [PATCH 169/189] [MultiverseMJCF] Add texture to xml generated from meshes. --- src/pycram/object_descriptors/mjcf.py | 32 +++++++++++++++++++++------ 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 686abe1e6..dd1f39259 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -2,23 +2,24 @@ import numpy as np import rospy +from dm_control import mjcf from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, Dict, Tuple -from dm_control import mjcf +from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ + SphereVisualShape, MeshVisualShape from ..datastructures.enums import JointType, MJCFGeomType, MJCFJointType from ..datastructures.pose import Pose from ..description import JointDescription as AbstractJointDescription, \ LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription -from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ - SphereVisualShape, MeshVisualShape from ..failures import MultiplePossibleTipLinks try: - from multiverse_parser import Configuration, Factory, InertiaSource + from multiverse_parser import Configuration, Factory, InertiaSource, GeomBuilder from multiverse_parser import (WorldBuilder, GeomType, GeomProperty, - MeshProperty) + MeshProperty, + MaterialProperty) from multiverse_parser import MjcfExporter from pxr import Usd, UsdGeom except ImportError: @@ -50,7 +51,7 @@ def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: if mjcf_geometry.type == MJCFGeomType.BOX.value: return BoxVisualShape(Color(), [0, 0, 0], mjcf_geometry.size) if mjcf_geometry.type == MJCFGeomType.CYLINDER.value: - return CylinderVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0], mjcf_geometry.size[1]*2) + return CylinderVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0], mjcf_geometry.size[1] * 2) if mjcf_geometry.type == MJCFGeomType.SPHERE.value: return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0]) if mjcf_geometry.type == MJCFGeomType.MESH.value: @@ -162,7 +163,7 @@ def friction(self) -> float: class ObjectFactory(Factory): - def __init__(self, object_name: str, file_path: str, config: Configuration): + def __init__(self, object_name: str, file_path: str, config: Configuration, texture_type: str = "png"): super().__init__(file_path, config) self._world_builder = WorldBuilder(usd_file_path=self.tmp_usd_file_path) @@ -177,16 +178,33 @@ def __init__(self, object_name: str, file_path: str, config: Configuration): mesh_path = mesh_prim.GetPath() mesh_property = MeshProperty.from_mesh_file_path(mesh_file_path=tmp_usd_mesh_file_path, mesh_path=mesh_path) + # mesh_property._texture_coordinates = None # TODO: See if needed otherwise remove it. geom_property = GeomProperty(geom_type=GeomType.MESH, is_visible=False, is_collidable=True) geom_builder = body_builder.add_geom(geom_name=f"SM_{object_name}_mesh_{idx}", geom_property=geom_property) geom_builder.add_mesh(mesh_name=mesh_name, mesh_property=mesh_property) + + # Add texture if available + texture_file_path = file_path.replace(pathlib.Path(file_path).suffix, f".{texture_type}") + if pathlib.Path(texture_file_path).exists(): + self.add_material_with_texture(geom_builder=geom_builder, material_name=f"M_{object_name}_{idx}", + texture_file_path=texture_file_path) + geom_builder.build() body_builder.compute_and_set_inertial(inertia_source=InertiaSource.FROM_COLLISION_MESH) + @staticmethod + def add_material_with_texture(geom_builder: GeomBuilder, material_name: str, texture_file_path: str): + material_property = MaterialProperty(diffuse_color=texture_file_path, + opacity=None, + emissive_color=None, + specular_color=None) + geom_builder.add_material(material_name=material_name, + material_property=material_property) + def export_to_mjcf(self, output_file_path: str): exporter = MjcfExporter(self, output_file_path) exporter.build() From f8f8955551c824a2758ca4e1da5c393f732cab10 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 12 Sep 2024 16:35:37 +0200 Subject: [PATCH 170/189] [MultiverseMJCF] Added an optional config variable to indicate whether to clear the cache at start or not. --- src/pycram/cache_manager.py | 14 +++++++++++--- src/pycram/config/multiverse_conf.py | 2 ++ src/pycram/config/world_conf.py | 5 +++++ src/pycram/datastructures/world.py | 8 ++++++-- src/pycram/object_descriptors/urdf.py | 4 ++-- src/pycram/worlds/multiverse.py | 13 +++++++++---- src/pycram/worlds/multiverse_extras/helpers.py | 6 ++++-- 7 files changed, 39 insertions(+), 13 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index cbd3cac31..ca829fc72 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -15,16 +15,23 @@ class CacheManager: The CacheManager is responsible for caching object description files and managing the cache directory. """ - def __init__(self, cache_dir: str, data_directory: List[str]): + cache_cleared: bool = False + """ + A class variable that indicates whether the cache directory has been cleared. + """ + + def __init__(self, cache_dir: str, data_directory: List[str], clear_cache: bool = True): """ Initialize the CacheManager. :param cache_dir: The directory where the cached files are stored. :param data_directory: The directory where all resource files are stored. + :param clear_cache: If True, the cache directory will be cleared. """ self.cache_dir = cache_dir self.data_directories = data_directory - self.clear_cache() + if clear_cache: + self.clear_cache() def clear_cache(self): """ @@ -32,6 +39,7 @@ def clear_cache(self): """ self.delete_cache_dir() self.create_cache_dir_if_not_exists() + self.cache_cleared = True def delete_cache_dir(self): """ @@ -105,7 +113,7 @@ def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: :param object_description: The object description of the file. :return: True if there already exists a cached file, False in any other case. """ - return True if self.check_with_extension(path) else self.check_without_extension(path, object_description) + return self.check_with_extension(path) or self.check_without_extension(path, object_description) def check_with_extension(self, path: str) -> bool: """ diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 19ba149bb..917c22730 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -58,6 +58,8 @@ Whether to use the physics simulator state when restoring or saving the world state. """ +world_conf.clear_cache_at_start = False + job_handling: world_conf.JobHandling = world_conf.JobHandling(let_pycram_move_attached_objects=False, let_pycram_handle_spawning=False) diff --git a/src/pycram/config/world_conf.py b/src/pycram/config/world_conf.py index 93f101644..c4e34bf8b 100644 --- a/src/pycram/config/world_conf.py +++ b/src/pycram/config/world_conf.py @@ -22,6 +22,11 @@ Global reference for the cache directory, this is used to cache the description files of the robot and the objects. """ +clear_cache_at_start: bool = True +""" +Whether to clear the cache directory at the start. +""" + prospection_world_prefix: str = "prospection_" """ The prefix for the prospection world name. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 6eec27f10..46d6cafcd 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -66,7 +66,7 @@ class World(StateEntity, ABC): the URDF with the name of the URDF on the parameter server. """ - cache_manager: CacheManager = CacheManager(conf.cache_dir, [conf.resources_path]) + cache_manager: CacheManager = CacheManager(conf.cache_dir, [conf.resources_path], False) """ Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ @@ -111,7 +111,7 @@ class World(StateEntity, ABC): """ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, - **config_kwargs): + clear_cache: bool = False, **config_kwargs): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -121,11 +121,15 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ "GUI" :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. :param simulation_frequency: The frequency of the simulation in Hz. + :param clear_cache: Whether to clear the cache directory. :param config_kwargs: Additional configuration parameters. """ StateEntity.__init__(self) + if clear_cache or (conf.clear_cache_at_start and not self.cache_manager.cache_cleared): + self.cache_manager.clear_cache() + # Parse config_kwargs for key, value in config_kwargs.items(): setattr(self, key, value) diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index a6ede80f3..d4bd2dfb9 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -281,7 +281,7 @@ def generate_from_description_file(self, path: str, save_path: str, make_mesh_pa urdf_string = self.remove_error_tags(urdf_string) urdf_string = self.fix_link_attributes(urdf_string) try: - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) urdf_string = self.fix_missing_inertial(urdf_string) except rospkg.ResourceNotFound as e: rospy.logerr(f"Could not find resource package linked in this URDF") @@ -291,7 +291,7 @@ def generate_from_description_file(self, path: str, save_path: str, make_mesh_pa def generate_from_parameter_server(self, name: str, save_path: str) -> None: urdf_string = rospy.get_param(name) - urdf_string = self.replace_ros_package_references_to_absolute_paths(urdf_string) + urdf_string = self.replace_relative_references_with_absolute_paths(urdf_string) urdf_string = self.fix_missing_inertial(urdf_string) self.write_description_to_file(urdf_string, save_path) diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 8a53450bb..b59dc525f 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -77,7 +77,8 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, simulation_frequency: float = conf.simulation_frequency, - simulation_name: str = "pycram_test"): + simulation_name: str = "pycram_test", + clear_cache: bool = False): """ Initialize the Multiverse Socket and the PyCram World. @@ -85,11 +86,12 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, :param is_prospection: Whether the world is prospection or not. :param simulation_frequency: The frequency of the simulation. :param simulation_name: The name of the simulation. + :param clear_cache: Whether to clear the cache or not. """ self.latest_save_id: Optional[int] = None self.saved_simulator_states: Dict = {} - self._make_sure_multiverse_resources_are_added() + self._make_sure_multiverse_resources_are_added(clear_cache=clear_cache) if Multiverse.simulation is None: if simulation_name is None: @@ -144,12 +146,15 @@ def _init_constraint_and_object_id_name_map_collections(self): def _init_world(self, mode: WorldMode): pass - def _make_sure_multiverse_resources_are_added(self): + def _make_sure_multiverse_resources_are_added(self, clear_cache: bool = False): """ Add the multiverse resources to the pycram world resources, and change the data directory and cache manager. + + :param clear_cache: Whether to clear the cache or not. """ if not self.added_multiverse_resources: - World.cache_manager.clear_cache() + if clear_cache: + World.cache_manager.clear_cache() World.add_resource_path(conf.resources_path, prepend=True) World.change_cache_dir_path(conf.resources_path) self.added_multiverse_resources = True diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py index 43189402c..de83b592f 100644 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -43,8 +43,10 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti rospy.logwarn("Multiverse resources path not found.") return None if multiverse_resources is not None: - if f'mjcf/{xml_name}' in os.listdir(robot_folder): - return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, 'mjcf', xml_name) + list_dir = os.listdir(robot_folder) + if 'mjcf' in list_dir: + if xml_name in os.listdir(robot_folder + '/mjcf'): + return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, 'mjcf', xml_name) elif xml_name in os.listdir(robot_folder): return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, xml_name) return None From bbb7418efde22aee08f1cc8e7f3bd508c907b9d1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 12 Sep 2024 16:50:58 +0200 Subject: [PATCH 171/189] [MultiverseMJCF] cleaning --- src/pycram/worlds/multiverse_extras/helpers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py index de83b592f..8286e5c86 100644 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ b/src/pycram/worlds/multiverse_extras/helpers.py @@ -46,9 +46,9 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti list_dir = os.listdir(robot_folder) if 'mjcf' in list_dir: if xml_name in os.listdir(robot_folder + '/mjcf'): - return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, 'mjcf', xml_name) + return os.path.join(robot_folder, 'mjcf', xml_name) elif xml_name in os.listdir(robot_folder): - return os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name, xml_name) + return os.path.join(robot_folder, xml_name) return None From f42a3997d420ac0bd2fd70766c69098ed35fcc64 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 12 Sep 2024 17:43:20 +0200 Subject: [PATCH 172/189] [MultiverseMJCF] cleaning, doc, and merged multiverse helpers, dataclasses and enums with pycram ones. --- src/pycram/config/multiverse_conf.py | 2 +- src/pycram/datastructures/dataclasses.py | 39 ++++++ src/pycram/datastructures/enums.py | 70 +++++++++++ src/pycram/datastructures/world.py | 20 +++- src/pycram/external_interfaces/giskard.py | 1 - src/pycram/failures.py | 9 +- src/pycram/helper.py | 96 ++++++++++++++- src/pycram/robot_description.py | 2 +- .../robot_descriptions/pr2_description.py | 2 +- .../robot_descriptions/tiago_description.py | 2 +- src/pycram/validation/error_checkers.py | 111 ++++++++++++++++-- src/pycram/validation/goal_validator.py | 64 ++++++---- src/pycram/worlds/multiverse.py | 5 +- .../multiverse_communication/clients.py | 6 +- .../worlds/multiverse_communication/socket.py | 2 +- .../multiverse_datastructures/__init__.py | 0 .../multiverse_datastructures/dataclasses.py | 42 ------- .../worlds/multiverse_datastructures/enums.py | 72 ------------ .../worlds/multiverse_extras/__init__.py | 0 .../worlds/multiverse_extras/exceptions.py | 6 - .../worlds/multiverse_extras/helpers.py | 92 --------------- test/test_multiverse.py | 2 +- 22 files changed, 387 insertions(+), 258 deletions(-) delete mode 100644 src/pycram/worlds/multiverse_datastructures/__init__.py delete mode 100644 src/pycram/worlds/multiverse_datastructures/dataclasses.py delete mode 100644 src/pycram/worlds/multiverse_datastructures/enums.py delete mode 100644 src/pycram/worlds/multiverse_extras/__init__.py delete mode 100644 src/pycram/worlds/multiverse_extras/exceptions.py delete mode 100644 src/pycram/worlds/multiverse_extras/helpers.py diff --git a/src/pycram/config/multiverse_conf.py b/src/pycram/config/multiverse_conf.py index 917c22730..708e86768 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/src/pycram/config/multiverse_conf.py @@ -4,7 +4,7 @@ from . import world_conf as world_conf from ..description import ObjectDescription -from ..worlds.multiverse_extras.helpers import find_multiverse_resources_path +from ..helper import find_multiverse_resources_path from ..object_descriptors.mjcf import ObjectDescription as MJCF # Multiverse Configuration diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 05fb2a2c9..4106b5869 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -661,3 +661,42 @@ def get_axes(self) -> Dict[str, Point]: return {self.translation_x: Point(1, 0, 0), self.translation_y: Point(0, 1, 0), self.angular_z: Point(0, 0, 1)} + + +@dataclass +class MultiverseMetaData: + """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" + world_name: str = "world" + simulation_name: str = "cram" + length_unit: str = "m" + angle_unit: str = "rad" + mass_unit: str = "kg" + time_unit: str = "s" + handedness: str = "rhs" + + +@dataclass +class RayResult: + """ + A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the + distance from the ray origin to the intersection point. + """ + body_name: str + distance: float + + def intersected(self) -> bool: + """ + Check if the ray intersects with a body. + return: Whether the ray intersects with a body. + """ + return self.distance >= 0 and self.body_name != "" + + +@dataclass +class MultiverseContactPoint: + """ + A dataclass to store the contact point returned from Multiverse. + """ + body_name: str + contact_force: List[float] + contact_torque: List[float] diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 61d245cb1..84d5ccaad 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -2,6 +2,8 @@ from enum import Enum, auto +from ..failures import UnsupportedJointType + class ExecutionType(Enum): """Enum for Execution Process Module types.""" @@ -201,3 +203,71 @@ class MJCFJointType(Enum): SLIDE = "slide" HINGE = "hinge" FIXED = "fixed" # Added for compatibility with PyCRAM, but not a real joint type in MuJoCo. + + +class MultiverseAPIName(Enum): + """ + Enum for the different APIs of the Multiverse. + """ + GET_CONTACT_BODIES = "get_contact_bodies" + GET_CONSTRAINT_EFFORT = "get_constraint_effort" + ATTACH = "attach" + DETACH = "detach" + GET_RAYS = "get_rays" + EXIST = "exist" + PAUSE = "pause" + UNPAUSE = "unpause" + SAVE = "save" + LOAD = "load" + + +class MultiverseProperty(Enum): + def __str__(self): + return self.value + + +class MultiverseBodyProperty(MultiverseProperty): + """ + Enum for the different properties of a body the Multiverse. + """ + POSITION = "position" + ORIENTATION = "quaternion" + RELATIVE_VELOCITY = "relative_velocity" + + +class MultiverseJointProperty(MultiverseProperty): + pass + + +class MultiverseJointPosition(MultiverseJointProperty): + """ + Enum for the Position names of the different joint types in the Multiverse. + """ + REVOLUTE_JOINT_POSITION = "joint_rvalue" + PRISMATIC_JOINT_POSITION = "joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointPosition': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointPosition.REVOLUTE_JOINT_POSITION + elif joint_type == JointType.PRISMATIC: + return MultiverseJointPosition.PRISMATIC_JOINT_POSITION + else: + raise UnsupportedJointType(joint_type) + + +class MultiverseJointCMD(MultiverseJointProperty): + """ + Enum for the Command names of the different joint types in the Multiverse. + """ + REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" + PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" + + @classmethod + def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointCMD': + if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: + return MultiverseJointCMD.REVOLUTE_JOINT_CMD + elif joint_type == JointType.PRISMATIC: + return MultiverseJointCMD.PRISMATIC_JOINT_CMD + else: + raise UnsupportedJointType(joint_type) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 46d6cafcd..19fc1b25b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -624,6 +624,8 @@ def update_all_objects_poses(self) -> None: def get_object_pose(self, obj: Object) -> Pose: """ Get the pose of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. """ pass @@ -631,6 +633,8 @@ def get_object_pose(self, obj: Object) -> Pose: def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: """ Get the poses of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. """ pass @@ -638,6 +642,8 @@ def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List[float]]: """ Get the positions of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. """ pass @@ -645,6 +651,8 @@ def get_multiple_object_positions(self, objects: List[Object]) -> Dict[str, List def get_object_position(self, obj: Object) -> List[float]: """ Get the position of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. """ pass @@ -652,6 +660,8 @@ def get_object_position(self, obj: Object) -> List[float]: def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, List[float]]: """ Get the orientations of multiple objects in the world frame from the current object poses in the simulator. + + :param objects: The objects. """ pass @@ -659,20 +669,22 @@ def get_multiple_object_orientations(self, objects: List[Object]) -> Dict[str, L def get_object_orientation(self, obj: Object) -> List[float]: """ Get the orientation of an object in the world frame from the current object pose in the simulator. + + :param obj: The object. """ pass @property def robot_virtual_joints(self) -> List[Joint]: """ - Return the virtual joints of the robot. + The virtual joints of the robot. """ return [self.robot.joints[name] for name in self.robot_virtual_joints_names] @property def robot_virtual_joints_names(self) -> List[str]: """ - Return the virtual joints of the robot. + The names of the virtual joints of the robot. """ return self.robot_description.virtual_move_base_joints.names @@ -757,6 +769,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b Set the positions of multiple joints of an articulated object. NOTE: It is recommended to use the validate_multiple_joint_positions decorator to validate the joint positions for the implementation of this method. + :param joint_positions: A dictionary with joint objects as keys and joint positions as values. :return: True if the set was successful, False otherwise. """ @@ -766,6 +779,8 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b def get_multiple_joint_positions(self, joints: List[Joint]) -> Dict[str, float]: """ Get the positions of multiple joints of an articulated object. + + :param joints: The joints as a list of Joint objects. """ pass @@ -777,6 +792,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. NOTE: It is recommended to use the validate_object_pose decorator to validate the object pose for the implementation of this method. + :param obj: The object. :param pose: The new pose as a Pose object. :return: True if the reset was successful, False otherwise. diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index b86d004dc..3ae16e973 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -22,7 +22,6 @@ try: from giskardpy.python_interface.old_python_interface import OldGiskardWrapper as GiskardWrapper from giskard_msgs.msg import WorldBody, MoveResult, CollisionEntry - #from giskard_msgs.srv import UpdateWorldRequest, UpdateWorld, UpdateWorldResponse, RegisterGroupResponse except ModuleNotFoundError as e: rospy.logwarn("Failed to import Giskard messages, the real robot will not be available") diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 2fc2a59f8..736adf8ee 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -3,7 +3,8 @@ from typing_extensions import TYPE_CHECKING, List if TYPE_CHECKING: - from pycram.world_concepts.world_object import Object + from .world_concepts.world_object import Object + from .datastructures.enums import JointType class PlanFailure(Exception): @@ -462,3 +463,9 @@ class ObjectDescriptionUndefined(Exception): def __init__(self, object_name: str): super().__init__(f"Object description for object {object_name} is not defined, eith a path or a description" f"object should be provided.") + + +class UnsupportedJointType(Exception): + def __init__(self, joint_type: 'JointType'): + super().__init__(f"Unsupported joint type: {joint_type}") + diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 73cf77dbc..015062651 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -3,6 +3,13 @@ Classes: Singleton -- implementation of singleton metaclass """ +import os + +import rospy +from typing_extensions import Dict, Optional +import xml.etree.ElementTree as ET + + class Singleton(type): """ Metaclass for singletons @@ -16,4 +23,91 @@ class Singleton(type): def __call__(cls, *args, **kwargs): if cls not in cls._instances: cls._instances[cls] = super(Singleton, cls).__call__(*args, **kwargs) - return cls._instances[cls] \ No newline at end of file + return cls._instances[cls] + + +def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: + """ + Parse the actuator elements from an MJCF file. + :param file_path: The path to the MJCF file. + """ + tree = ET.parse(file_path) + root = tree.getroot() + + joint_actuators = {} + + # Iterate through all actuator elements + for actuator in root.findall(".//actuator/*"): + name = actuator.get('name') + joint = actuator.get('joint') + if name and joint: + joint_actuators[joint] = name + + return joint_actuators + + +def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: + """ + Get the path to the MJCF file of a robot. + :param robot_relative_dir: The relative directory of the robot in the Multiverse resources/robots directory. + :param robot_name: The name of the robot. + :param xml_name: The name of the XML file of the robot. + :return: The path to the MJCF file of the robot if it exists, otherwise None. + """ + xml_name = xml_name if xml_name is not None else robot_name + if '.xml' not in xml_name: + xml_name = xml_name + '.xml' + multiverse_resources = find_multiverse_resources_path() + try: + robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name) + except TypeError: + rospy.logwarn("Multiverse resources path not found.") + return None + if multiverse_resources is not None: + list_dir = os.listdir(robot_folder) + if 'mjcf' in list_dir: + if xml_name in os.listdir(robot_folder + '/mjcf'): + return os.path.join(robot_folder, 'mjcf', xml_name) + elif xml_name in os.listdir(robot_folder): + return os.path.join(robot_folder, xml_name) + return None + + +def find_multiverse_resources_path() -> Optional[str]: + """ + Find the path to the Multiverse resources directory. + """ + # Get the path to the Multiverse installation + multiverse_path = find_multiverse_path() + + # Check if the path to the Multiverse installation was found + if multiverse_path: + # Construct the path to the resources directory + resources_path = os.path.join(multiverse_path, 'resources') + + # Check if the resources directory exists + if os.path.exists(resources_path): + return resources_path + + return None + + +def find_multiverse_path() -> Optional[str]: + """ + Find the path to the Multiverse installation. + """ + # Get the value of PYTHONPATH environment variable + pythonpath = os.getenv('PYTHONPATH') + + # Check if PYTHONPATH is set + if pythonpath: + # Split the PYTHONPATH into individual paths using the platform-specific path separator + paths = pythonpath.split(os.pathsep) + + # Iterate through each path and check if 'Multiverse' is in it + for path in paths: + if 'multiverse' in path: + multiverse_path = path.split('multiverse')[0] + return multiverse_path + 'multiverse' + + return None diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 9ad707f82..46a7eed4a 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -8,7 +8,7 @@ from .datastructures.enums import Arms, Grasp, GripperState, GripperType, JointType from .object_descriptors.urdf import ObjectDescription as URDFObject from .utils import suppress_stdout_stderr -from .worlds.multiverse_extras.helpers import parse_mjcf_actuators +from .helper import parse_mjcf_actuators class RobotDescriptionManager: diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index fc0c68736..f87083840 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -4,7 +4,7 @@ from ..datastructures.enums import Arms, Grasp, GripperState, GripperType import rospkg -from ..worlds.multiverse_extras.helpers import get_robot_mjcf_path +from ..helper import get_robot_mjcf_path rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "pr2" + '.urdf' diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index d190cd4c4..bff284557 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -4,7 +4,7 @@ from ..datastructures.enums import GripperState, Arms, Grasp from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription -from ..worlds.multiverse_extras.helpers import get_robot_mjcf_path +from ..helper import get_robot_mjcf_path rospack = rospkg.RosPack() filename = rospack.get_path('pycram') + '/resources/robots/' + "tiago_dual" + '.urdf' diff --git a/src/pycram/validation/error_checkers.py b/src/pycram/validation/error_checkers.py index 34f596330..f594b6bd6 100644 --- a/src/pycram/validation/error_checkers.py +++ b/src/pycram/validation/error_checkers.py @@ -3,11 +3,11 @@ import numpy as np from tf.transformations import quaternion_multiply, quaternion_inverse -from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable, TYPE_CHECKING +from typing_extensions import List, Union, Optional, Any, Sized, Iterable as T_Iterable, TYPE_CHECKING, Tuple -from pycram.datastructures.enums import JointType +from ..datastructures.enums import JointType if TYPE_CHECKING: - from pycram.datastructures.pose import Pose + from ..datastructures.pose import Pose class ErrorChecker(ABC): @@ -18,6 +18,7 @@ class ErrorChecker(ABC): def __init__(self, acceptable_error: Union[float, T_Iterable[float]], is_iterable: Optional[bool] = False): """ Initialize the error checker. + :param acceptable_error: The acceptable error. :param is_iterable: Whether the error is iterable (i.e. list of errors). """ @@ -43,6 +44,9 @@ def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[floa tile_to_match: Optional[Sized] = None,) -> None: """ Update the acceptable error with a new value, and tile it to match the length of the error if needed. + + :param new_acceptable_error: The new acceptable error. + :param tile_to_match: The iterable to match the length of the error with. """ if new_acceptable_error is not None: self.acceptable_error = new_acceptable_error @@ -52,6 +56,7 @@ def update_acceptable_error(self, new_acceptable_error: Optional[T_Iterable[floa def update_tiled_acceptable_error(self, tile_to_match: Sized) -> None: """ Tile the acceptable error to match the length of the error. + :param tile_to_match: The object to match the length of the error. :return: The tiled acceptable error. """ @@ -62,6 +67,7 @@ def update_tiled_acceptable_error(self, tile_to_match: Sized) -> None: def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: """ Calculate the error between two values. + :param value_1: The first value. :param value_2: The second value. :return: The error between the two values. @@ -71,6 +77,7 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[floa def calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float]]: """ Calculate the error between two values. + :param value_1: The first value. :param value_2: The second value. :return: The error between the two values. @@ -83,6 +90,7 @@ def calculate_error(self, value_1: Any, value_2: Any) -> Union[float, List[float def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: """ Check if the error is acceptable. + :param value_1: The first value. :param value_2: The second value. :return: Whether the error is acceptable. @@ -100,13 +108,22 @@ def is_error_acceptable(self, value_1: Any, value_2: Any) -> bool: class PoseErrorChecker(ErrorChecker): - def __init__(self, acceptable_error: Union[float, T_Iterable[float]] = (1e-3, np.pi / 180), + def __init__(self, acceptable_error: Union[Tuple[float], T_Iterable[Tuple[float]]] = (1e-3, np.pi / 180), is_iterable: Optional[bool] = False): + """ + Initialize the pose error checker. + + :param acceptable_error: The acceptable pose error (position error, orientation error). + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> List[float]: """ Calculate the error between two poses. + + :param value_1: The first pose. + :param value_2: The second pose. """ return calculate_pose_error(value_1, value_2) @@ -114,11 +131,21 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> List[float]: class PositionErrorChecker(ErrorChecker): def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the position error checker. + + :param acceptable_error: The acceptable position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ Calculate the error between two positions. + + :param value_1: The first position. + :param value_2: The second position. + :return: The error between the two positions. """ return calculate_position_error(value_1, value_2) @@ -126,11 +153,21 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class OrientationErrorChecker(ErrorChecker): def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + """ + Initialize the orientation error checker. + + :param acceptable_error: The acceptable orientation error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ Calculate the error between two quaternions. + + :param value_1: The first quaternion. + :param value_2: The second quaternion. + :return: The error between the two quaternions. """ return calculate_orientation_error(value_1, value_2) @@ -138,11 +175,21 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class SingleValueErrorChecker(ErrorChecker): def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the single value error checker. + + :param acceptable_error: The acceptable error between two values. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. """ return abs(value_1 - value_2) @@ -150,23 +197,44 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class RevoluteJointPositionErrorChecker(SingleValueErrorChecker): def __init__(self, acceptable_error: Optional[float] = np.pi / 180, is_iterable: Optional[bool] = False): + """ + Initialize the revolute joint position error checker. + + :param acceptable_error: The acceptable revolute joint position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) class PrismaticJointPositionErrorChecker(SingleValueErrorChecker): def __init__(self, acceptable_error: Optional[float] = 1e-3, is_iterable: Optional[bool] = False): + """ + Initialize the prismatic joint position error checker. + + :param acceptable_error: The acceptable prismatic joint position error. + :param is_iterable: Whether the error is iterable (i.e. list of errors). + """ super().__init__(acceptable_error, is_iterable) class IterableErrorChecker(ErrorChecker): def __init__(self, acceptable_error: Optional[T_Iterable[float]] = None): + """ + Initialize the iterable error checker. + + :param acceptable_error: The acceptable error between two values. + """ super().__init__(acceptable_error, True) def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error between the two values. """ return abs(value_1 - value_2) @@ -174,6 +242,12 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: class MultiJointPositionErrorChecker(IterableErrorChecker): def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_Iterable[float]] = None): + """ + Initialize the multi-joint position error checker. + + :param joint_types: The types of the joints. + :param acceptable_error: The acceptable error between two joint positions. + """ self.joint_types = joint_types if acceptable_error is None: acceptable_error = [np.pi/180 if jt == JointType.REVOLUTE else 1e-3 for jt in joint_types] @@ -182,6 +256,10 @@ def __init__(self, joint_types: List[JointType], acceptable_error: Optional[T_It def _calculate_error(self, value_1: Any, value_2: Any) -> float: """ Calculate the error between two joint positions. + + :param value_1: The first joint position. + :param value_2: The second joint position. + :return: The error between the two joint positions. """ return calculate_joint_position_error(value_1, value_2) @@ -189,7 +267,10 @@ def _calculate_error(self, value_1: Any, value_2: Any) -> float: def calculate_pose_error(pose_1: 'Pose', pose_2: 'Pose') -> List[float]: """ Calculate the error between two poses. - return: The error between the two poses. + + :param pose_1: The first pose. + :param pose_2: The second pose. + :return: The error between the two poses. """ return [calculate_position_error(pose_1.position_as_list(), pose_2.position_as_list()), calculate_orientation_error(pose_1.orientation_as_list(), pose_2.orientation_as_list())] @@ -198,7 +279,10 @@ def calculate_pose_error(pose_1: 'Pose', pose_2: 'Pose') -> List[float]: def calculate_position_error(position_1: List[float], position_2: List[float]) -> float: """ Calculate the error between two positions. - return: The error between the two positions. + + :param position_1: The first position. + :param position_2: The second position. + :return: The error between the two positions. """ return np.linalg.norm(np.array(position_1) - np.array(position_2)) @@ -206,7 +290,10 @@ def calculate_position_error(position_1: List[float], position_2: List[float]) - def calculate_orientation_error(quat_1: List[float], quat_2: List[float]) -> float: """ Calculate the error between two quaternions. - return: The error between the two quaternions. + + :param quat_1: The first quaternion. + :param quat_2: The second quaternion. + :return: The error between the two quaternions. """ return calculate_angle_between_quaternions(quat_1, quat_2) @@ -214,7 +301,10 @@ def calculate_orientation_error(quat_1: List[float], quat_2: List[float]) -> flo def calculate_joint_position_error(joint_position_1: float, joint_position_2: float) -> float: """ Calculate the error between two joint positions. - return: The error between the two joint positions. + + :param joint_position_1: The first joint position. + :param joint_position_2: The second joint position. + :return: The error between the two joint positions. """ return abs(joint_position_1 - joint_position_2) @@ -223,9 +313,10 @@ def is_error_acceptable(error: Union[float, T_Iterable[float]], acceptable_error: Union[float, T_Iterable[float]]) -> bool: """ Check if the error is acceptable. + :param error: The error. :param acceptable_error: The acceptable error. - return: Whether the error is acceptable. + :return: Whether the error is acceptable. """ if isinstance(error, Iterable): return all([error_i <= acceptable_error_i for error_i, acceptable_error_i in zip(error, acceptable_error)]) @@ -236,6 +327,7 @@ def is_error_acceptable(error: Union[float, T_Iterable[float]], def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float]) -> float: """ Calculates the angle between two quaternions. + :param quat_1: The first quaternion. :param quat_2: The second quaternion. :return: A float value that represents the angle between the two quaternions. @@ -250,6 +342,7 @@ def calculate_angle_between_quaternions(quat_1: List[float], quat_2: List[float] def calculate_quaternion_difference(quat_1: List[float], quat_2: List[float]) -> List[float]: """ Calculates the quaternion difference. + :param quat_1: The quaternion of the object at the first time step. :param quat_2: The quaternion of the object at the second time step. :return: A list of float values that represent the quaternion difference. diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 1a3d40a02..0b9f7cd17 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -2,17 +2,17 @@ import numpy as np import rospy -from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING +from typing_extensions import Any, Callable, Optional, Union, Iterable, Dict, TYPE_CHECKING, Tuple -from pycram.datastructures.enums import JointType -from pycram.validation.error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ +from ..datastructures.enums import JointType +from .error_checkers import ErrorChecker, PoseErrorChecker, PositionErrorChecker, \ OrientationErrorChecker, SingleValueErrorChecker if TYPE_CHECKING: - from pycram.datastructures.world import World - from pycram.world_concepts.world_object import Object - from pycram.datastructures.pose import Pose - from pycram.description import ObjectDescription + from ..datastructures.world import World + from ..world_concepts.world_object import Object + from ..datastructures.pose import Pose + from ..description import ObjectDescription Joint = ObjectDescription.Joint Link = ObjectDescription.Link @@ -34,6 +34,7 @@ def __init__(self, error_checker: ErrorChecker, current_value_getter: OptionalAr acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the goal validator. + :param error_checker: The error checker. :param current_value_getter: The current value getter function which takes an optional input and returns the current value. @@ -55,6 +56,7 @@ def register_goal_and_wait_until_achieved(self, goal_value: Any, time_per_read: Optional[float] = 0.01) -> None: """ Register the goal value and wait until the target is reached. + :param goal_value: The goal value. :param current_value_getter_input: The values that are used as input to the current value getter. :param initial_value: The initial value. @@ -69,6 +71,7 @@ def wait_until_goal_is_achieved(self, max_wait_time: Optional[float] = 2, time_per_read: Optional[float] = 0.01) -> None: """ Wait until the target is reached. + :param max_wait_time: The maximum time to wait. :param time_per_read: The time to wait between each read. """ @@ -104,7 +107,7 @@ def reset(self) -> None: @property def _acceptable_error(self) -> np.ndarray: """ - Get the acceptable error. + The acceptable error. """ if self.error_checker.is_iterable: return self.tiled_acceptable_error @@ -114,14 +117,14 @@ def _acceptable_error(self) -> np.ndarray: @property def acceptable_error(self) -> np.ndarray: """ - Get the acceptable error. + The acceptable error. """ return self.error_checker.acceptable_error @property def tiled_acceptable_error(self) -> Optional[np.ndarray]: """ - Get the tiled acceptable error. + The tiled acceptable error. """ return self.error_checker.tiled_acceptable_error @@ -131,6 +134,7 @@ def register_goal(self, goal_value: Any, acceptable_error: Optional[Union[float, Iterable[float]]] = None): """ Register the goal value. + :param goal_value: The goal value. :param current_value_getter_input: The values that are used as input to the current value getter. :param initial_value: The initial value. @@ -146,6 +150,9 @@ def register_goal(self, goal_value: Any, def update_initial_error(self, goal_value: Any, initial_value: Optional[Any] = None) -> None: """ Calculate the initial error. + + :param goal_value: The goal value. + :param initial_value: The initial value. """ if initial_value is None: self.initial_error: np.ndarray = self.current_error @@ -155,7 +162,7 @@ def update_initial_error(self, goal_value: Any, initial_value: Optional[Any] = N @property def current_value(self) -> Any: """ - Get the current value. + The current value of the monitored variable. """ if self.current_value_getter_input is not None: return self.current_value_getter(self.current_value_getter_input) @@ -165,20 +172,24 @@ def current_value(self) -> Any: @property def current_error(self) -> np.ndarray: """ - Calculate the current error. + The current error. """ return self.calculate_error(self.goal_value, self.current_value) def calculate_error(self, value_1: Any, value_2: Any) -> np.ndarray: """ Calculate the error between two values. + + :param value_1: The first value. + :param value_2: The second value. + :return: The error. """ return np.array(self.error_checker.calculate_error(value_1, value_2)).flatten() @property def percentage_of_goal_achieved(self) -> float: """ - Calculate the percentage of goal achieved. + The relative (relative to the acceptable error) achieved percentage of goal. """ percent_array = 1 - self.relative_current_error / self.relative_initial_error percent_array_filtered = percent_array[self.relative_initial_error > self._acceptable_error] @@ -190,7 +201,7 @@ def percentage_of_goal_achieved(self) -> float: @property def actual_percentage_of_goal_achieved(self) -> float: """ - Calculate the percentage of goal achieved. + The percentage of goal achieved. """ percent_array = 1 - self.current_error / np.maximum(self.initial_error, 1e-3) percent_array_filtered = percent_array[self.initial_error > self._acceptable_error] @@ -202,14 +213,14 @@ def actual_percentage_of_goal_achieved(self) -> float: @property def relative_current_error(self) -> np.ndarray: """ - Get the relative current error. + The relative current error (relative to the acceptable error). """ return self.get_relative_error(self.current_error, threshold=0) @property def relative_initial_error(self) -> np.ndarray: """ - Get the relative initial error. + The relative initial error (relative to the acceptable error). """ return np.maximum(self.initial_error, 1e-3) @@ -217,8 +228,10 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n """ Get the relative error by comparing the error with the acceptable error and filtering out the errors that are less than the threshold. + :param error: The error. :param threshold: The threshold. + :return: The relative error. """ return np.maximum(error - self._acceptable_error, threshold) @@ -226,7 +239,6 @@ def get_relative_error(self, error: Any, threshold: Optional[float] = 1e-3) -> n def goal_achieved(self) -> bool: """ Check if the goal is achieved. - return: Whether the goal is achieved. """ if self.acceptable_percentage_of_goal_achieved is None: return self.is_current_error_acceptable @@ -237,7 +249,6 @@ def goal_achieved(self) -> bool: def is_current_error_acceptable(self) -> bool: """ Check if the error is acceptable. - return: Whether the error is acceptable. """ return self.error_checker.is_error_acceptable(self.current_value, self.goal_value) @@ -248,11 +259,12 @@ class PoseGoalValidator(GoalValidator): """ def __init__(self, current_pose_getter: OptionalArgCallable = None, - acceptable_error: Union[float, Iterable[float]] = (1e-3, np.pi / 180), + acceptable_error: Union[Tuple[float], Iterable[Tuple[float]]] = (1e-3, np.pi / 180), acceptable_percentage_of_goal_achieved: Optional[float] = 0.8, is_iterable: Optional[bool] = False): """ Initialize the pose goal validator. + :param current_pose_getter: The current pose getter function which takes an optional input and returns the current pose. :param acceptable_error: The acceptable error. @@ -268,10 +280,11 @@ class MultiPoseGoalValidator(PoseGoalValidator): """ def __init__(self, current_poses_getter: OptionalArgCallable = None, - acceptable_error: Union[float, Iterable[float]] = (1e-2, 5 * np.pi / 180), + acceptable_error: Union[Tuple[float], Iterable[Tuple[float]]] = (1e-2, 5 * np.pi / 180), acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-pose goal validator. + :param current_poses_getter: The current poses getter function which takes an optional input and returns the current poses. :param acceptable_error: The acceptable error. @@ -292,6 +305,7 @@ def __init__(self, current_position_getter: OptionalArgCallable = None, is_iterable: Optional[bool] = False): """ Initialize the position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and returns the current position. :param acceptable_error: The acceptable error. @@ -312,6 +326,7 @@ def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and returns the current positions. :param acceptable_error: The acceptable error. @@ -332,6 +347,7 @@ def __init__(self, current_orientation_getter: OptionalArgCallable = None, is_iterable: Optional[bool] = False): """ Initialize the orientation goal validator. + :param current_orientation_getter: The current orientation getter function which takes an optional input and returns the current orientation. :param acceptable_error: The acceptable error. @@ -352,6 +368,7 @@ def __init__(self, current_orientations_getter: OptionalArgCallable = None, acceptable_percentage_of_goal_achieved: Optional[float] = 0.8): """ Initialize the multi-orientation goal validator. + :param current_orientations_getter: The current orientations getter function which takes an optional input and returns the current orientations. :param acceptable_error: The acceptable error. @@ -374,6 +391,7 @@ def __init__(self, current_position_getter: OptionalArgCallable = None, is_iterable: bool = False): """ Initialize the joint position goal validator. + :param current_position_getter: The current position getter function which takes an optional input and returns the current position. :param acceptable_error: The acceptable error. @@ -393,6 +411,7 @@ def register_goal(self, goal_value: Any, joint_type: JointType, acceptable_error: Optional[float] = None): """ Register the goal value. + :param goal_value: The goal value. :param joint_type: The joint type (e.g. REVOLUTE, PRISMATIC). :param current_value_getter_input: The values that are used as input to the current value getter. @@ -417,6 +436,7 @@ def __init__(self, current_positions_getter: OptionalArgCallable = None, acceptable_percentage_of_goal_achieved: float = 0.8): """ Initialize the multi-joint position goal validator. + :param current_positions_getter: The current positions getter function which takes an optional input and returns the current positions. :param acceptable_error: The acceptable error. @@ -442,6 +462,7 @@ def register_goal(self, goal_value: Any, joint_type: Iterable[JointType], def validate_object_pose(pose_setter_func): """ A decorator to validate the object pose. + :param pose_setter_func: The function to set the pose of the object. """ @@ -462,6 +483,7 @@ def wrapper(world: 'World', obj: 'Object', pose: 'Pose'): def validate_multiple_object_poses(pose_setter_func): """ A decorator to validate multiple object poses. + :param pose_setter_func: The function to set multiple poses of the objects. """ @@ -483,6 +505,7 @@ def wrapper(world: 'World', object_poses: Dict['Object', 'Pose']): def validate_joint_position(position_setter_func): """ A decorator to validate the joint position. + :param position_setter_func: The function to set the joint position. """ @@ -507,6 +530,7 @@ def validate_multiple_joint_positions(position_setter_func): as in multiverse the virtual joints take command velocities and not positions, so after their goals are set, they are zeroed thus can't be validated. (They are actually validated by the robot pose in case of virtual move base joints) + :param position_setter_func: The function to set the joint positions. """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index b59dc525f..83c7196df 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -8,11 +8,10 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI -from .multiverse_datastructures.enums import MultiverseBodyProperty, MultiverseJointPosition, \ - MultiverseJointCMD from ..config import multiverse_conf as conf, world_conf from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint -from ..datastructures.enums import WorldMode, JointType, ObjectType +from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ + MultiverseJointCMD from ..datastructures.pose import Pose from ..datastructures.world import World from ..description import Link, Joint, ObjectDescription diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 754d7f987..93ae9af38 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -8,10 +8,10 @@ from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData -from ..multiverse_datastructures.dataclasses import RayResult, MultiverseContactPoint -from ..multiverse_datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, - MultiverseProperty as Property) from ...config import multiverse_conf as conf +from ...datastructures.dataclasses import RayResult, MultiverseContactPoint +from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, + MultiverseProperty as Property) from ...datastructures.pose import Pose from ...datastructures.world import World from ...world_concepts.constraints import Constraint diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index f63624e2d..202a7e4ca 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -6,7 +6,7 @@ from multiverse_client_pybind import MultiverseClientPybind # noqa from typing_extensions import Optional, List, Dict, Callable, TypeVar -from ..multiverse_datastructures.dataclasses import MultiverseMetaData +from ...datastructures.dataclasses import MultiverseMetaData from ...config import multiverse_conf as conf T = TypeVar("T") diff --git a/src/pycram/worlds/multiverse_datastructures/__init__.py b/src/pycram/worlds/multiverse_datastructures/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/pycram/worlds/multiverse_datastructures/dataclasses.py b/src/pycram/worlds/multiverse_datastructures/dataclasses.py deleted file mode 100644 index 85f94b2c1..000000000 --- a/src/pycram/worlds/multiverse_datastructures/dataclasses.py +++ /dev/null @@ -1,42 +0,0 @@ -from dataclasses import dataclass - -from typing_extensions import List - - -@dataclass -class MultiverseMetaData: - """Meta data for the Multiverse Client, the simulation_name should be non-empty and unique for each simulation""" - world_name: str = "world" - simulation_name: str = "cram" - length_unit: str = "m" - angle_unit: str = "rad" - mass_unit: str = "kg" - time_unit: str = "s" - handedness: str = "rhs" - - -@dataclass -class RayResult: - """ - A dataclass to store the ray result. The ray result contains the body name that the ray intersects with and the - distance from the ray origin to the intersection point. - """ - body_name: str - distance: float - - def intersected(self) -> bool: - """ - Check if the ray intersects with a body. - return: Whether the ray intersects with a body. - """ - return self.distance >= 0 and self.body_name != "" - - -@dataclass -class MultiverseContactPoint: - """ - A dataclass to store the contact point returned from Multiverse. - """ - body_name: str - contact_force: List[float] - contact_torque: List[float] diff --git a/src/pycram/worlds/multiverse_datastructures/enums.py b/src/pycram/worlds/multiverse_datastructures/enums.py deleted file mode 100644 index b30647954..000000000 --- a/src/pycram/worlds/multiverse_datastructures/enums.py +++ /dev/null @@ -1,72 +0,0 @@ -from enum import Enum - -from pycram.datastructures.enums import JointType -from ..multiverse_extras.exceptions import UnsupportedJointType - - -class MultiverseAPIName(Enum): - """ - Enum for the different APIs of the Multiverse. - """ - GET_CONTACT_BODIES = "get_contact_bodies" - GET_CONSTRAINT_EFFORT = "get_constraint_effort" - ATTACH = "attach" - DETACH = "detach" - GET_RAYS = "get_rays" - EXIST = "exist" - PAUSE = "pause" - UNPAUSE = "unpause" - SAVE = "save" - LOAD = "load" - - -class MultiverseProperty(Enum): - def __str__(self): - return self.value - - -class MultiverseBodyProperty(MultiverseProperty): - """ - Enum for the different properties of a body the Multiverse. - """ - POSITION = "position" - ORIENTATION = "quaternion" - RELATIVE_VELOCITY = "relative_velocity" - - -class MultiverseJointProperty(MultiverseProperty): - pass - - -class MultiverseJointPosition(MultiverseJointProperty): - """ - Enum for the Position names of the different joint types in the Multiverse. - """ - REVOLUTE_JOINT_POSITION = "joint_rvalue" - PRISMATIC_JOINT_POSITION = "joint_tvalue" - - @classmethod - def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointPosition': - if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: - return MultiverseJointPosition.REVOLUTE_JOINT_POSITION - elif joint_type == JointType.PRISMATIC: - return MultiverseJointPosition.PRISMATIC_JOINT_POSITION - else: - raise UnsupportedJointType(joint_type) - - -class MultiverseJointCMD(MultiverseJointProperty): - """ - Enum for the Command names of the different joint types in the Multiverse. - """ - REVOLUTE_JOINT_CMD = "cmd_joint_rvalue" - PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" - - @classmethod - def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointCMD': - if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: - return MultiverseJointCMD.REVOLUTE_JOINT_CMD - elif joint_type == JointType.PRISMATIC: - return MultiverseJointCMD.PRISMATIC_JOINT_CMD - else: - raise UnsupportedJointType(joint_type) diff --git a/src/pycram/worlds/multiverse_extras/__init__.py b/src/pycram/worlds/multiverse_extras/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/pycram/worlds/multiverse_extras/exceptions.py b/src/pycram/worlds/multiverse_extras/exceptions.py deleted file mode 100644 index bca03b173..000000000 --- a/src/pycram/worlds/multiverse_extras/exceptions.py +++ /dev/null @@ -1,6 +0,0 @@ -from pycram.datastructures.enums import JointType - - -class UnsupportedJointType(Exception): - def __init__(self, joint_type: 'JointType'): - super().__init__(f"Unsupported joint type: {joint_type}") diff --git a/src/pycram/worlds/multiverse_extras/helpers.py b/src/pycram/worlds/multiverse_extras/helpers.py deleted file mode 100644 index 8286e5c86..000000000 --- a/src/pycram/worlds/multiverse_extras/helpers.py +++ /dev/null @@ -1,92 +0,0 @@ -import os - -import rospy -from typing_extensions import Optional, List, Dict -import xml.etree.ElementTree as ET - - -def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: - """ - Parse the actuator elements from an MJCF file. - :param file_path: The path to the MJCF file. - """ - tree = ET.parse(file_path) - root = tree.getroot() - - joint_actuators = {} - - # Iterate through all actuator elements - for actuator in root.findall(".//actuator/*"): - name = actuator.get('name') - joint = actuator.get('joint') - if name and joint: - joint_actuators[joint] = name - - return joint_actuators - - -def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: - """ - Get the path to the MJCF file of a robot. - :param robot_relative_dir: The relative directory of the robot in the Multiverse resources/robots directory. - :param robot_name: The name of the robot. - :param xml_name: The name of the XML file of the robot. - :return: The path to the MJCF file of the robot if it exists, otherwise None. - """ - xml_name = xml_name if xml_name is not None else robot_name - if '.xml' not in xml_name: - xml_name = xml_name + '.xml' - multiverse_resources = find_multiverse_resources_path() - try: - robot_folder = os.path.join(multiverse_resources, 'robots', robot_relative_dir, robot_name) - except TypeError: - rospy.logwarn("Multiverse resources path not found.") - return None - if multiverse_resources is not None: - list_dir = os.listdir(robot_folder) - if 'mjcf' in list_dir: - if xml_name in os.listdir(robot_folder + '/mjcf'): - return os.path.join(robot_folder, 'mjcf', xml_name) - elif xml_name in os.listdir(robot_folder): - return os.path.join(robot_folder, xml_name) - return None - - -def find_multiverse_resources_path() -> Optional[str]: - """ - Find the path to the Multiverse resources directory. - """ - # Get the path to the Multiverse installation - multiverse_path = find_multiverse_path() - - # Check if the path to the Multiverse installation was found - if multiverse_path: - # Construct the path to the resources directory - resources_path = os.path.join(multiverse_path, 'resources') - - # Check if the resources directory exists - if os.path.exists(resources_path): - return resources_path - - return None - - -def find_multiverse_path() -> Optional[str]: - """ - Find the path to the Multiverse installation. - """ - # Get the value of PYTHONPATH environment variable - pythonpath = os.getenv('PYTHONPATH') - - # Check if PYTHONPATH is set - if pythonpath: - # Split the PYTHONPATH into individual paths using the platform-specific path separator - paths = pythonpath.split(os.pathsep) - - # Iterate through each path and check if 'Multiverse' is in it - for path in paths: - if 'multiverse' in path: - multiverse_path = path.split('multiverse')[0] - return multiverse_path + 'multiverse' - - return None diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ac152aa80..19e902214 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -14,7 +14,7 @@ from pycram.robot_description import RobotDescriptionManager from pycram.world_concepts.world_object import Object from pycram.validation.error_checkers import calculate_angle_between_quaternions -from pycram.worlds.multiverse_extras.helpers import get_robot_mjcf_path, parse_mjcf_actuators +from pycram.helper import get_robot_mjcf_path, parse_mjcf_actuators multiverse_installed = True try: From 0eb328e27bd8e316ea6bdce03033b12f79928f65 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Sep 2024 09:49:55 +0200 Subject: [PATCH 173/189] [MultiverseMJCF] unused import in test. --- test/test_multiverse.py | 1 - 1 file changed, 1 deletion(-) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 19e902214..ff9ed06e9 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import os import unittest -from time import sleep import numpy as np import psutil From c4daacc2848b406f6f411834fabea63ff5e58320 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Sep 2024 11:15:49 +0200 Subject: [PATCH 174/189] [MultiverseDev] moved config package to root of the pycram pacakge --- {src/pycram/config => config}/__init__.py | 0 {src/pycram/config => config}/multiverse_conf.py | 6 +++--- {src/pycram/config => config}/world_conf.py | 6 +++--- test/test_multiverse.py | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) rename {src/pycram/config => config}/__init__.py (100%) rename {src/pycram/config => config}/multiverse_conf.py (92%) rename {src/pycram/config => config}/world_conf.py (95%) diff --git a/src/pycram/config/__init__.py b/config/__init__.py similarity index 100% rename from src/pycram/config/__init__.py rename to config/__init__.py diff --git a/src/pycram/config/multiverse_conf.py b/config/multiverse_conf.py similarity index 92% rename from src/pycram/config/multiverse_conf.py rename to config/multiverse_conf.py index 708e86768..af3f2a4ed 100644 --- a/src/pycram/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -3,9 +3,9 @@ from typing_extensions import Type from . import world_conf as world_conf -from ..description import ObjectDescription -from ..helper import find_multiverse_resources_path -from ..object_descriptors.mjcf import ObjectDescription as MJCF +from pycram.description import ObjectDescription +from pycram.helper import find_multiverse_resources_path +from pycram.object_descriptors.mjcf import ObjectDescription as MJCF # Multiverse Configuration resources_path: str = find_multiverse_resources_path() diff --git a/src/pycram/config/world_conf.py b/config/world_conf.py similarity index 95% rename from src/pycram/config/world_conf.py rename to config/world_conf.py index c4e34bf8b..220571d50 100644 --- a/src/pycram/config/world_conf.py +++ b/config/world_conf.py @@ -3,10 +3,10 @@ from dataclasses import dataclass from typing_extensions import Tuple, Type -from ..description import ObjectDescription -from ..object_descriptors.urdf import ObjectDescription as URDF +from pycram.description import ObjectDescription +from pycram.object_descriptors.urdf import ObjectDescription as URDF -resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') +resources_path = os.path.join(os.path.dirname(__file__), '..', 'resources') """ Global reference for the resources path, this is used to search for the description files of the robot and the objects. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index ff9ed06e9..cafe364fe 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -304,7 +304,7 @@ def test_get_object_contact_points(self): cup = self.spawn_cup([1, 1, 0.2]) # 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.2) + self.multiverse.simulate(0.3) contact_points = self.multiverse.get_object_contact_points(cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From 4ed74fef7290d99b2028386c64ac7a0b76b053d5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Sep 2024 13:25:03 +0200 Subject: [PATCH 175/189] [MultiverseDev] cleaning and renaming. --- src/pycram/datastructures/pose.py | 2 ++ src/pycram/world_concepts/world_object.py | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 71530fff7..1ca3a228f 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -269,9 +269,11 @@ def almost_equal(self, other: Pose, position_tolerance_in_meters: float = 1e-3, tolerance. The position tolerance is given in meters and the orientation tolerance in degrees. The position error is calculated as the euclidian distance between the positions and the orientation error as the angle between the quaternions. + :param other: The other Pose which should be compared :param position_tolerance_in_meters: The tolerance for the position in meters :param orientation_tolerance_in_degrees: The tolerance for the orientation in degrees + :return: True if the Poses are almost equal, False otherwise """ error = calculate_pose_error(self, other) return error[0] <= position_tolerance_in_meters and error[1] <= orientation_tolerance_in_degrees * math.pi / 180 diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 073ad8f17..fb1c2edd3 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -37,7 +37,7 @@ class Object(WorldEntity): Represents a spawned Object in the World. """ - prospection_world_prefix: str = "prospection/" + tf_prospection_world_prefix: str = "prospection/" """ The prefix for the tf frame of objects in the prospection world. """ @@ -100,7 +100,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.id = self._spawn_object_and_get_id() - self.tf_frame = (self.prospection_world_prefix if self.world.is_prospection_world else "") + self.name + self.tf_frame = (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name self._init_joint_name_and_id_map() self._init_link_name_and_id_map() @@ -881,8 +881,8 @@ def get_attachment_transform_with_object(self, attachment: Attachment, child_obj raise WorldMismatchErrorBetweenObjects(self, child_object) att_transform = attachment.parent_to_child_transform.copy() if self.world.is_prospection_world and not attachment.parent_object.world.is_prospection_world: - att_transform.frame = self.prospection_world_prefix + att_transform.frame - att_transform.child_frame_id = self.prospection_world_prefix + att_transform.child_frame_id + att_transform.frame = self.tf_prospection_world_prefix + att_transform.frame + att_transform.child_frame_id = self.tf_prospection_world_prefix + att_transform.child_frame_id return att_transform @property From 20937937daff1ada907951753df990eba2e7c55d Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Sep 2024 13:44:15 +0200 Subject: [PATCH 176/189] [MultiverseDev] check_object_exists implementation is now up to the world type. added With UseProspectionWorld when get object for prospection object. --- src/pycram/datastructures/world.py | 11 +++++++---- src/pycram/worlds/multiverse.py | 2 +- test/test_multiverse.py | 6 +++--- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 19fc1b25b..b7167875f 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -244,12 +244,14 @@ def _init_goal_validators(self): acceptable_prismatic_joint_position_error=self.acceptable_prismatic_joint_position_error, acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) - def check_object_exists(self, obj: Object): + def check_object_exists(self, obj: Object) -> bool: """ - Check if the object exists in the simulator and issues a warning if it does not. + Check if the object exists in the simulator. + :param obj: The object to check. + :return: True if the object is in the world, False otherwise. """ - return obj not in self.objects + raise NotImplementedError @abstractmethod def _init_world(self, mode: WorldMode): @@ -1160,7 +1162,8 @@ def get_object_for_prospection_object(self, prospection_object: Object) -> Objec :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. """ - return self.world_sync.get_world_object(prospection_object) + with UseProspectionWorld(): + return self.world_sync.get_world_object(prospection_object) def remove_all_objects(self, exclude_objects: Optional[List[Object]] = None) -> None: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 83c7196df..6fdb9673b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -672,7 +672,7 @@ def set_realtime(self, real_time: bool) -> None: def set_gravity(self, gravity_vector: List[float]) -> None: logging.warning("set_gravity is not implemented in Multiverse") - def check_object_exists_in_multiverse(self, obj: Object) -> bool: + def check_object_exists(self, obj: Object) -> bool: """ Check if the object exists in the Multiverse world. diff --git a/test/test_multiverse.py b/test/test_multiverse.py index cafe364fe..4ba127d15 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -127,11 +127,11 @@ def test_remove_object(self): milk = self.spawn_milk([1, 1, 0.1]) milk.remove() self.assertTrue(milk not in self.multiverse.objects) - self.assertFalse(self.multiverse.check_object_exists_in_multiverse(milk)) + self.assertFalse(self.multiverse.check_object_exists(milk)) def test_check_object_exists(self): milk = self.spawn_milk([1, 1, 0.1]) - self.assertTrue(self.multiverse.check_object_exists_in_multiverse(milk)) + self.assertTrue(self.multiverse.check_object_exists(milk)) def test_set_position(self): milk = self.spawn_milk([1, 1, 0.1]) @@ -318,7 +318,7 @@ def test_get_contact_points_between_two_objects(self): cup = self.spawn_cup([1, 1, 0.2]) # 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.2) + self.multiverse.simulate(0.3) contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup) self.assertIsInstance(contact_points, ContactPointsList) self.assertEqual(len(contact_points), 1) From 9234065aa927add90fea7f3efb08a07bfee2a25e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 13 Sep 2024 17:55:06 +0200 Subject: [PATCH 177/189] [MultiverseDev] In process of refactoring use of configuration files. --- config/multiverse_conf.py | 96 +++++++------- config/world_conf.py | 123 ++++++++---------- src/pycram/datastructures/world.py | 93 ++++--------- src/pycram/description.py | 8 +- src/pycram/world_concepts/world_object.py | 10 +- src/pycram/world_reasoning.py | 8 +- src/pycram/worlds/multiverse.py | 65 ++++----- .../multiverse_communication/clients.py | 7 +- test/test_multiverse.py | 41 +++--- 9 files changed, 194 insertions(+), 257 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index af3f2a4ed..65a7912a2 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -2,66 +2,68 @@ from typing_extensions import Type -from . import world_conf as world_conf +from .world_conf import WorldConfig from pycram.description import ObjectDescription from pycram.helper import find_multiverse_resources_path from pycram.object_descriptors.mjcf import ObjectDescription as MJCF -# Multiverse Configuration -resources_path: str = find_multiverse_resources_path() -""" -The path to the Multiverse resources directory. -""" +class MultiverseConfig(WorldConfig): + # Multiverse Configuration + WorldConfig.resources_path = find_multiverse_resources_path() + """ + The path to the Multiverse resources directory. + """ -# Multiverse Socket Configuration -HOST: str = "tcp://127.0.0.1" -SERVER_HOST: str = HOST -SERVER_PORT: str = "7000" -BASE_CLIENT_PORT: int = 9000 + # Multiverse Socket Configuration + HOST: str = "tcp://127.0.0.1" + SERVER_HOST: str = HOST + SERVER_PORT: str = "7000" + BASE_CLIENT_PORT: int = 9000 -# Multiverse Client Configuration -READER_MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) -""" -The maximum wait time for the data in seconds. -""" + # Multiverse Client Configuration + READER_MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = datetime.timedelta(milliseconds=1000) + """ + The maximum wait time for the data in seconds. + """ -# Multiverse Simulation Configuration -simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) -simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) + # Multiverse Simulation Configuration + simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) + simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) -simulation_wait_time_factor: float = 1.0 -""" -The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for -the time taken by the simulation to process the request, this depends on the computational power of the machine -running the simulation. -""" + simulation_wait_time_factor: float = 1.0 + """ + The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for + the time taken by the simulation to process the request, this depends on the computational power of the machine + running the simulation. + """ -use_bullet_mode: bool = True -""" -If True, the simulation will always be in paused state unless the simulate() function is called, this behaves -similar to bullet_world which uses the bullet physics engine. -""" + use_bullet_mode: bool = True + """ + If True, the simulation will always be in paused state unless the simulate() function is called, this behaves + similar to bullet_world which uses the bullet physics engine. + """ -use_controller: bool = False -""" -Only used when use_bullet_mode is False. This turns on the controller for the robot joints. -""" + use_controller: bool = False + use_controller = use_controller and not use_bullet_mode + """ + Only used when use_bullet_mode is False. This turns on the controller for the robot joints. + """ -default_description_type: Type[ObjectDescription] = MJCF -""" -The default description type for the objects. -""" + default_description_type: Type[ObjectDescription] = MJCF + """ + The default description type for the objects. + """ -use_physics_simulator_state: bool = True -""" -Whether to use the physics simulator state when restoring or saving the world state. -""" + use_physics_simulator_state: bool = True + """ + Whether to use the physics simulator state when restoring or saving the world state. + """ -world_conf.clear_cache_at_start = False + WorldConfig.clear_cache_at_start = False -job_handling: world_conf.JobHandling = world_conf.JobHandling(let_pycram_move_attached_objects=False, - let_pycram_handle_spawning=False) + WorldConfig.let_pycram_move_attached_objects = False + WorldConfig.let_pycram_handle_spawning = False -error_tolerance: world_conf.ErrorTolerance = world_conf.ErrorTolerance(acceptable_position_error=2e-2, - acceptable_prismatic_joint_position_error=2e-2) + WorldConfig.position_tolerance = 2e-2 + WorldConfig.prismatic_joint_position_tolerance = 2e-2 diff --git a/config/world_conf.py b/config/world_conf.py index 220571d50..280cc90dd 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -6,57 +6,57 @@ from pycram.description import ObjectDescription from pycram.object_descriptors.urdf import ObjectDescription as URDF -resources_path = os.path.join(os.path.dirname(__file__), '..', 'resources') -""" -Global reference for the resources path, this is used to search for the description files of the robot and - the objects. -""" - -cache_dir_name: str = 'cached' -""" -The name of the cache directory. -""" - -cache_dir: str = os.path.join(resources_path, cache_dir_name) -""" -Global reference for the cache directory, this is used to cache the description files of the robot and the objects. -""" - -clear_cache_at_start: bool = True -""" -Whether to clear the cache directory at the start. -""" - -prospection_world_prefix: str = "prospection_" -""" -The prefix for the prospection world name. -""" - -update_poses_from_sim_on_get: bool = True -""" -Whether to update the poses from the simulator when getting the object poses. -""" - -DEBUG: bool = False -""" -Whether to use in debug mode. (This is used to print debug messages, plot images, etc.) -""" - -default_description_type: Type[ObjectDescription] = URDF -""" -The default description type for the objects. -""" - -use_physics_simulator_state: bool = False -""" -Whether to use the physics simulator state when restoring or saving the world state. -Currently with PyBullet, this causes a bug where ray_test does not work correctly after restoring the state using the -simulator, so it is recommended to set this to False in PyBullet. -""" - - -@dataclass -class JobHandling: + +class WorldConfig: + + resources_path = os.path.join(os.path.dirname(__file__), '..', 'resources') + """ + Global reference for the resources path, this is used to search for the description files of the robot and + the objects. + """ + + cache_dir_name: str = 'cached' + """ + The name of the cache directory. + """ + + cache_dir: str = os.path.join(resources_path, cache_dir_name) + """ + Global reference for the cache directory, this is used to cache the description files of the robot and the objects. + """ + + clear_cache_at_start: bool = True + """ + Whether to clear the cache directory at the start. + """ + + prospection_world_prefix: str = "prospection_" + """ + The prefix for the prospection world name. + """ + + update_poses_from_sim_on_get: bool = True + """ + Whether to update the poses from the simulator when getting the object poses. + """ + + DEBUG: bool = False + """ + Whether to use in debug mode. (This is used to print debug messages, plot images, etc.) + """ + + default_description_type: Type[ObjectDescription] = URDF + """ + The default description type for the objects. + """ + + use_physics_simulator_state: bool = False + """ + Whether to use the physics simulator state when restoring or saving the world state. + Currently with PyBullet, this causes a bug where ray_test does not work correctly after restoring the state using the + simulator, so it is recommended to set this to False in PyBullet. + """ + let_pycram_move_attached_objects: bool = True let_pycram_handle_spawning: bool = True let_pycram_handle_world_sync: bool = True @@ -65,16 +65,10 @@ class JobHandling: and the world synchronization. """ - def as_dict(self): - return self.__dict__ - - -@dataclass -class ErrorTolerance: - acceptable_position_error: float = 1e-2 - acceptable_orientation_error: float = 10 * math.pi / 180 - acceptable_prismatic_joint_position_error: float = 1e-2 - acceptable_revolute_joint_position_error: float = 5 * math.pi / 180 + position_tolerance: float = 1e-2 + orientation_tolerance: float = 10 * math.pi / 180 + prismatic_joint_position_tolerance: float = 1e-2 + revolute_joint_position_tolerance: float = 5 * math.pi / 180 """ The acceptable error for the position and orientation of an object/link, and the joint positions. """ @@ -91,8 +85,5 @@ class ErrorTolerance: """ @property - def acceptable_pose_error(self) -> Tuple[float, float]: - return self.acceptable_position_error, self.acceptable_orientation_error - - def as_dict(self): - return self.__dict__ + def pose_tolerance(self) -> Tuple[float, float]: + return self.position_tolerance, self.orientation_tolerance diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index b7167875f..37890a00b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -13,7 +13,7 @@ from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type from ..cache_manager import CacheManager -from ..config import world_conf as conf +from ..config.world_conf import WorldConfig from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, MultiBody, VisualShape, BoxVisualShape, CylinderVisualShape, SphereVisualShape, @@ -47,6 +47,11 @@ class World(StateEntity, ABC): current_world which is managed by the World class itself. """ + conf: Type[WorldConfig] = WorldConfig + """ + The configurations of the world, the default configurations are defined in world_conf.py in the config folder. + """ + simulation_frequency: float """ Global reference for the simulation frequency (Hz), used in calculating the equivalent real time in the simulation. @@ -54,10 +59,10 @@ class World(StateEntity, ABC): current_world: Optional[World] = None """ - Global reference to the currently used World, usually this is the - graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the - prospection world. In this way you can comfortably use the current_world, which should point towards the World - used at the moment. + Global reference to the currently used World, usually this is the + graphical one. However, if you are inside a UseProspectionWorld() environment the current_world points to the + prospection world. In this way you can comfortably use the current_world, which should point towards the World + used at the moment. """ robot: Optional[Object] = None @@ -71,47 +76,8 @@ class World(StateEntity, ABC): Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ - prospection_world_prefix: str = conf.prospection_world_prefix - """ - The prefix for the prospection world name. - """ - - let_pycram_move_attached_objects: bool = conf.JobHandling.let_pycram_move_attached_objects - let_pycram_handle_spawning: bool = conf.JobHandling.let_pycram_handle_spawning - let_pycram_handle_world_sync: bool = conf.JobHandling.let_pycram_handle_world_sync - """ - Whether to let PyCRAM handle the movement of attached objects, the spawning of objects, - and the world synchronization. - """ - - update_poses_from_sim_on_get: bool = conf.update_poses_from_sim_on_get - """ - Whether to update the poses from the simulator when getting the object poses. - """ - - acceptable_position_error: float = conf.ErrorTolerance.acceptable_position_error - acceptable_orientation_error: float = conf.ErrorTolerance.acceptable_orientation_error - acceptable_pose_error: Tuple[float, float] = conf.ErrorTolerance.acceptable_pose_error - acceptable_revolute_joint_position_error: float = conf.ErrorTolerance.acceptable_revolute_joint_position_error - acceptable_prismatic_joint_position_error: float = conf.ErrorTolerance.acceptable_prismatic_joint_position_error - use_percentage_of_goal: bool = conf.ErrorTolerance.use_percentage_of_goal - acceptable_percentage_of_goal: float = conf.ErrorTolerance.acceptable_percentage_of_goal - """ - The acceptable error for the position and orientation of an object/link. - """ - - raise_goal_validator_error: Optional[bool] = conf.ErrorTolerance.raise_goal_validator_error - """ - Whether to raise an error if the goals are not achieved. - """ - - default_description_type: Type[ObjectDescription] = conf.default_description_type - """ - The default description type for objects (e.g. URDF, MJCF, etc.). - """ - def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, - clear_cache: bool = False, **config_kwargs): + clear_cache: bool = False): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -122,19 +88,14 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. :param simulation_frequency: The frequency of the simulation in Hz. :param clear_cache: Whether to clear the cache directory. - :param config_kwargs: Additional configuration parameters. """ StateEntity.__init__(self) - if clear_cache or (conf.clear_cache_at_start and not self.cache_manager.cache_cleared): + if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): self.cache_manager.clear_cache() - # Parse config_kwargs - for key, value in config_kwargs.items(): - setattr(self, key, value) - - GoalValidator.raise_error = World.raise_goal_validator_error + GoalValidator.raise_error = World.conf.raise_goal_validator_error World.simulation_frequency = simulation_frequency if World.current_world is None: @@ -226,23 +187,23 @@ def _init_goal_validators(self): """ # Objects Pose goal validators - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.acceptable_pose_error, - self.acceptable_percentage_of_goal) + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.pose_tolerance, + self.conf.acceptable_percentage_of_goal) self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), - self.acceptable_pose_error, self.acceptable_percentage_of_goal) + self.conf.pose_tolerance, self.conf.acceptable_percentage_of_goal) # Joint Goal validators self.joint_position_goal_validator = JointPositionGoalValidator( self.get_joint_position, - acceptable_revolute_joint_position_error=self.acceptable_revolute_joint_position_error, - acceptable_prismatic_joint_position_error=self.acceptable_prismatic_joint_position_error, - acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator( lambda x: list(self.get_multiple_joint_positions(x).values()), - acceptable_revolute_joint_position_error=self.acceptable_revolute_joint_position_error, - acceptable_prismatic_joint_position_error=self.acceptable_prismatic_joint_position_error, - acceptable_percentage_of_goal_achieved=self.acceptable_percentage_of_goal) + acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance, + acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance, + acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal) def check_object_exists(self, obj: Object) -> bool: """ @@ -1000,14 +961,14 @@ 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 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(True) self._current_state = WorldState(simulator_state, self.object_states) return WorldState(self._current_state.simulator_state_id, self.object_states) @current_state.setter def current_state(self, state: WorldState) -> None: if self.current_state != state: - if conf.use_physics_simulator_state: + if self.conf.use_physics_simulator_state: self.restore_physics_simulator_state(state.simulator_state_id) else: for obj in self.objects: @@ -1140,7 +1101,7 @@ def change_cache_dir_path(cls, path: str) -> None: :param path: The new path for the cache directory. """ - cls.cache_manager.cache_dir = os.path.join(path, conf.cache_dir_name) + cls.cache_manager.cache_dir = os.path.join(path, cls.conf.cache_dir_name) def get_prospection_object_for_object(self, obj: Object) -> Object: """ @@ -1193,7 +1154,7 @@ def remove_saved_states(self) -> None: """ Remove all saved states of the World. """ - if conf.use_physics_simulator_state: + if self.conf.use_physics_simulator_state: for state_id in self.saved_states: self.remove_physics_simulator_state(state_id) else: @@ -1549,7 +1510,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 conf.use_physics_simulator_state: + if self.conf.use_physics_simulator_state: self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id) @property diff --git a/src/pycram/description.py b/src/pycram/description.py index b5f1f3148..ae4e73c3f 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -403,7 +403,7 @@ def pose(self) -> Pose: :return: A Pose object containing the pose of the link relative to the world frame. """ - if self.world.update_poses_from_sim_on_get: + if self.world.conf.update_poses_from_sim_on_get: self.update_pose() return self._current_pose @@ -505,8 +505,8 @@ def __init__(self, _id: int, obj: Object, is_virtual: Optional[bool] = False): ObjectEntity.__init__(self, _id, obj) JointDescription.__init__(self, joint_description.parsed_description, is_virtual) - self.acceptable_error = (self.world.acceptable_revolute_joint_position_error if self.type == JointType.REVOLUTE - else self.world.acceptable_prismatic_joint_position_error) + self.acceptable_error = (self.world.conf.revolute_joint_position_tolerance if self.type == JointType.REVOLUTE + else self.world.conf.prismatic_joint_position_tolerance) self._update_position() @property @@ -551,7 +551,7 @@ def child_link(self) -> Link: @property def position(self) -> float: - if self.world.update_poses_from_sim_on_get: + if self.world.conf.update_poses_from_sim_on_get: self._update_position() return self._current_position diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index fb1c2edd3..dd6b29ec4 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -128,7 +128,7 @@ def _resolve_description(self, path: Optional[str] = None, description: Optional if extension in self.extension_to_description_type: self.description = self.extension_to_description_type[extension]() elif extension in ObjectDescription.mesh_extensions: - self.description = self.world.default_description_type() + self.description = self.world.conf.default_description_type() else: raise UnsupportedFileExtension(self.name, path) @@ -303,7 +303,7 @@ def _spawn_object_and_get_id(self) -> int: if isinstance(self.description, GenericObjectDescription): return self.world.load_generic_object_and_get_id(self.description) - path = self.path if self.world.let_pycram_handle_spawning else self.name + path = self.path if self.world.conf.let_pycram_handle_spawning else self.name try: obj_id = self.world.load_object_and_get_id(path, self._current_pose, self.obj_type) @@ -710,7 +710,7 @@ def get_pose(self) -> Pose: :return: The current pose of this object """ - if self.world.update_poses_from_sim_on_get: + if self.world.conf.update_poses_from_sim_on_get: self.update_pose() return self._current_pose @@ -792,7 +792,7 @@ def current_state(self) -> ObjectState: The current state of this object as an ObjectState. """ return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), - self.joint_states.copy(), self.world.acceptable_pose_error) + self.joint_states.copy(), self.world.conf.pose_tolerance) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -959,7 +959,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - if not self.world.let_pycram_move_attached_objects: + if not World.conf.let_pycram_move_attached_objects: return if already_moved_objects is None: diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 0356c297b..10d3b8245 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -65,8 +65,8 @@ def contact( def get_visible_objects( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - plot_segmentation_mask: bool = conf.DEBUG) -> Tuple[np.ndarray, Pose]: - """ + plot_segmentation_mask: bool = World.conf.DEBUG) -> Tuple[np.ndarray, Pose]: + """W Return a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. :param camera_pose: The pose of the camera in world coordinate frame. @@ -97,7 +97,7 @@ def visible( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, threshold: float = 0.8, - plot_segmentation_mask: bool = conf.DEBUG) -> bool: + plot_segmentation_mask: bool = World.conf.DEBUG) -> bool: """ Checks if an object is visible from a given position. This will be achieved by rendering the object alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the @@ -141,7 +141,7 @@ def occluding( obj: Object, camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - plot_segmentation_mask: bool = conf.DEBUG) -> List[Object]: + plot_segmentation_mask: bool = World.conf.DEBUG) -> List[Object]: """ Lists all objects which are occluding the given object. This works similar to 'visible'. First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 6fdb9673b..3e8604293 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -4,11 +4,11 @@ import numpy as np import rospy from tf.transformations import quaternion_matrix -from typing_extensions import List, Dict, Optional, Union, Tuple, Type +from typing_extensions import List, Dict, Optional, Union, Tuple from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI -from ..config import multiverse_conf as conf, world_conf +from ..config.multiverse_conf import MultiverseConfig from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD @@ -17,11 +17,11 @@ from ..description import Link, Joint, ObjectDescription from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescription +from ..utils import RayTestUtils from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses from ..world_concepts.constraints import Constraint from ..world_concepts.world_object import Object -from ..utils import RayTestUtils class Multiverse(World): @@ -29,6 +29,11 @@ class Multiverse(World): This class implements an interface between Multiverse and PyCRAM. """ + conf: MultiverseConfig = MultiverseConfig + """ + The Multiverse configuration. + """ + supported_joint_types = (JointType.REVOLUTE, JointType.CONTINUOUS, JointType.PRISMATIC) """ A Tuple for the supported pycram joint types in Multiverse. @@ -45,29 +50,6 @@ class Multiverse(World): the multiverse configuration file). """ - use_bullet_mode: bool = conf.use_bullet_mode - """ - If True, the simulation will always be in paused state unless the simulate() function is called, this behaves - similar to bullet_world which uses the bullet physics engine. - """ - - use_controller: bool = conf.use_controller and not conf.use_bullet_mode - """ - Whether to use the controller for the robot joints or not. - """ - - simulation_wait_time_factor: float = conf.simulation_wait_time_factor - """ - The factor to multiply the simulation wait time with, this is used to adjust the simulation wait time to account for - the time taken by the simulation to process the request, this depends on the computational power of the machine - running the simulation. - """ - - default_description_type: Type[ObjectDescription] = conf.default_description_type - """ - The default description type for the objects. - """ - Object.extension_to_description_type[MJCF.get_file_extension()] = MJCF """ Add the MJCF description extension to the extension to description type mapping for the objects. @@ -98,12 +80,11 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, raise ValueError("Simulation name not provided") Multiverse.simulation = simulation_name - self.simulation = (self.prospection_world_prefix if is_prospection else "") + Multiverse.simulation - self.client_manager = MultiverseClientManager(self.simulation_wait_time_factor) + self.simulation = (self.conf.prospection_world_prefix if is_prospection else "") + Multiverse.simulation + self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) self._init_clients(is_prospection=is_prospection) - World.__init__(self, mode, is_prospection, simulation_frequency, **conf.job_handling.as_dict(), - **conf.error_tolerance.as_dict()) + World.__init__(self, mode, is_prospection, simulation_frequency) self._init_constraint_and_object_id_name_map_collections() @@ -112,7 +93,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if not self.is_prospection_world: self._spawn_floor() - if self.use_bullet_mode: + if self.conf.use_bullet_mode: self.api_requester.pause_simulation() def _init_clients(self, is_prospection: bool = False): @@ -131,7 +112,7 @@ def _init_clients(self, is_prospection: bool = False): self.api_requester: MultiverseAPI = self.client_manager.create_api_requester( self.simulation, is_prospection_world=is_prospection) - if self.use_controller: + if self.conf.use_controller: self.joint_controller: MultiverseController = self.client_manager.create_controller( is_prospection_world=is_prospection) @@ -154,8 +135,8 @@ def _make_sure_multiverse_resources_are_added(self, clear_cache: bool = False): if not self.added_multiverse_resources: if clear_cache: World.cache_manager.clear_cache() - World.add_resource_path(conf.resources_path, prepend=True) - World.change_cache_dir_path(conf.resources_path) + World.add_resource_path(self.conf.resources_path, prepend=True) + World.change_cache_dir_path(self.conf.resources_path) self.added_multiverse_resources = True def remove_multiverse_resources(self): @@ -163,8 +144,8 @@ def remove_multiverse_resources(self): Remove the multiverse resources from the pycram world resources. """ if self.added_multiverse_resources: - World.remove_resource_path(conf.resources_path) - World.change_cache_dir_path(world_conf.cache_dir) + World.remove_resource_path(self.conf.resources_path) + World.change_cache_dir_path(self.conf.cache_dir) self.added_multiverse_resources = False def _spawn_floor(self): @@ -242,7 +223,7 @@ def spawn_object(self, name: str, object_type: ObjectType, pose: Pose) -> None: :param object_type: The type of the object. :param pose: The pose of the object. """ - if object_type == ObjectType.ROBOT and self.use_controller: + if object_type == ObjectType.ROBOT and self.conf.use_controller: self.spawn_robot_with_controller(name, pose) else: self._set_body_pose(name, pose) @@ -279,7 +260,7 @@ def get_multiple_link_orientations(self, links: List[Link]) -> Dict[str, List[fl @validate_joint_position def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: - if self.use_controller and self.joint_has_actuator(joint): + if self.conf.use_controller and self.joint_has_actuator(joint): self._reset_joint_position_using_controller(joint, joint_position) else: self._set_multiple_joint_positions_without_controller({joint: joint_position}) @@ -305,7 +286,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b and use the controller to set the joint position if the joint is controlled. """ - if self.use_controller: + if self.conf.use_controller: controlled_joints = self.get_controlled_joints(list(joint_positions.keys())) if len(controlled_joints) > 0: controlled_joint_positions = {joint: joint_positions[joint] for joint in controlled_joints} @@ -343,7 +324,7 @@ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[J :param joint_positions: The dictionary of joints and positions. """ controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -515,7 +496,7 @@ def add_constraint(self, constraint: Constraint) -> int: logging.error("Only fixed constraints are supported in Multiverse") raise ValueError - if not self.let_pycram_move_attached_objects: + if not self.conf.let_pycram_move_attached_objects: self.api_requester.attach(constraint) return self._update_constraint_collection_and_get_latest_id(constraint) @@ -629,7 +610,7 @@ def step(self): """ Perform a simulation step in the simulator, this is useful when use_bullet_mode is True. """ - if self.use_bullet_mode: + if self.conf.use_bullet_mode: self.api_requester.unpause_simulation() sleep(self.simulation_time_step) self.api_requester.pause_simulation() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 93ae9af38..2740ab160 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -8,7 +8,7 @@ from typing_extensions import List, Dict, Tuple, Optional, Callable, Union from .socket import MultiverseSocket, MultiverseMetaData -from ...config import multiverse_conf as conf +from ...config.multiverse_conf import MultiverseConfig as Conf from ...datastructures.dataclasses import RayResult, MultiverseContactPoint from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) @@ -32,8 +32,9 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False, increase or decrease the wait time for the simulation. """ meta_data = MultiverseMetaData() - meta_data.simulation_name = (World.prospection_world_prefix if is_prospection_world else "") + name - meta_data.world_name = (World.prospection_world_prefix if is_prospection_world else "") + meta_data.world_name + meta_data.simulation_name = (Conf.prospection_world_prefix if is_prospection_world else "") + name + meta_data.world_name = ((Conf.prospection_world_prefix if is_prospection_world else "") + + meta_data.world_name) self.is_prospection_world = is_prospection_world super().__init__(port=str(port), meta_data=meta_data) self.simulation_wait_time_factor = simulation_wait_time_factor diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4ba127d15..eb10edb5e 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -100,16 +100,16 @@ def test_reset_world(self): milk = self.spawn_milk(set_position) milk.set_position(set_position) milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], set_position[:2], delta=self.multiverse.acceptable_position_error) + self.assert_list_is_equal(milk_position[:2], set_position[:2], delta=self.multiverse.conf.position_tolerance) self.multiverse.reset_world() milk_pose = milk.get_pose() self.assert_list_is_equal(milk_pose.position_as_list()[:2], milk.original_pose.position_as_list()[:2], - delta=self.multiverse.acceptable_position_error) + delta=self.multiverse.conf.position_tolerance) self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_spawn_robot_with_actuators_directly_from_multiverse(self): - if self.multiverse.use_controller: + if self.multiverse.conf.use_controller: robot_name = "tiago_dual" rdm = RobotDescriptionManager() rdm.load_description(robot_name) @@ -120,7 +120,7 @@ def test_spawn_object(self): self.assertIsInstance(milk, Object) milk_pose = milk.get_pose() self.assert_list_is_equal(milk_pose.position_as_list()[:2], [1, 1], - delta=self.multiverse.acceptable_position_error) + delta=self.multiverse.conf.position_tolerance) self.assert_orientation_is_equal(milk_pose.orientation_as_list(), milk.original_pose.orientation_as_list()) def test_remove_object(self): @@ -140,13 +140,13 @@ def test_set_position(self): milk.set_position(original_milk_position) milk_position = milk.get_position_as_list() self.assert_list_is_equal(milk_position[:2], original_milk_position[:2], - delta=self.multiverse.acceptable_position_error) + delta=self.multiverse.conf.position_tolerance) def test_update_position(self): milk = self.spawn_milk([1, 1, 0.1]) milk.update_pose() milk_position = milk.get_position_as_list() - self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.acceptable_position_error) + self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.conf.position_tolerance) def test_set_joint_position(self): if self.multiverse.robot is None: @@ -159,9 +159,9 @@ def test_set_joint_position(self): original_joint_position = robot.get_joint_position(joint) robot.set_joint_position(joint, original_joint_position + step) joint_position = robot.get_joint_position(joint) - if not self.multiverse.use_controller: - delta = self.multiverse.acceptable_position_error if joint_type == JointType.PRISMATIC \ - else self.multiverse.acceptable_orientation_error + if not self.multiverse.conf.use_controller: + delta = self.multiverse.conf.prismatic_joint_position_tolerance if joint_type == JointType.PRISMATIC \ + else self.multiverse.conf.revolute_joint_position_tolerance else: delta = 0.18 self.assertAlmostEqual(joint_position, original_joint_position + step, delta=delta) @@ -198,7 +198,7 @@ def test_set_robot_position(self): self.multiverse.robot.set_position(new_position) robot_position = self.multiverse.robot.get_position_as_list() self.assert_list_is_equal(robot_position[:2], new_position[:2], - delta=self.multiverse.acceptable_position_error) + delta=self.multiverse.conf.position_tolerance) self.tearDown() def test_set_robot_orientation(self): @@ -211,7 +211,7 @@ def test_set_robot_orientation(self): self.multiverse.robot.set_orientation(new_quaternion) robot_orientation = self.multiverse.robot.get_orientation_as_list() quaternion_difference = calculate_angle_between_quaternions(new_quaternion, robot_orientation) - self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.acceptable_orientation_error) + self.assertAlmostEqual(quaternion_difference, 0, delta=self.multiverse.conf.orientation_tolerance) def test_set_robot_pose(self): self.spawn_robot(orientation=quaternion_from_euler(0, 0, np.pi / 4)) @@ -234,8 +234,9 @@ def step_robot_pose(self, robot, position_step, angle_step, num_steps): new_pose = Pose(new_position, new_quaternion) self.multiverse.robot.set_pose(new_pose) robot_pose = self.multiverse.robot.get_pose() - self.assert_poses_are_equal(new_pose, robot_pose, position_delta=self.multiverse.acceptable_position_error, - orientation_delta=self.multiverse.acceptable_orientation_error) + self.assert_poses_are_equal(new_pose, robot_pose, + position_delta=self.multiverse.conf.position_tolerance, + orientation_delta=self.multiverse.conf.orientation_tolerance) def test_get_environment_pose(self): apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment.urdf") @@ -256,7 +257,7 @@ def test_attach_object(self): milk.set_position(milk_position) new_cup_position = cup.get_position_as_list() self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], - self.multiverse.acceptable_position_error) + self.multiverse.conf.position_tolerance) self.tearDown() def test_detach_object(self): @@ -275,9 +276,9 @@ def test_detach_object(self): new_milk_position = milk.get_position_as_list() new_cup_position = cup.get_position_as_list() self.assert_list_is_equal(new_milk_position[:2], milk_position[:2], - self.multiverse.acceptable_position_error) + self.multiverse.conf.position_tolerance) self.assert_list_is_equal(new_cup_position[:2], estimated_cup_position[:2], - self.multiverse.acceptable_position_error) + self.multiverse.conf.position_tolerance) self.tearDown() def test_attach_with_robot(self): @@ -382,22 +383,22 @@ def spawn_cup(position: List) -> Object: def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, position_delta: Optional[float] = None, orientation_delta: Optional[float] = None): if position_delta is None: - position_delta = self.multiverse.acceptable_position_error + position_delta = self.multiverse.conf.ErrorTolerance.position if orientation_delta is None: - orientation_delta = self.multiverse.acceptable_orientation_error + orientation_delta = self.multiverse.conf.orientation_tolerance self.assert_position_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) self.assert_orientation_is_equal(pose1.orientation_as_list(), pose2.orientation_as_list(), delta=orientation_delta) def assert_position_is_equal(self, position1: List[float], position2: List[float], delta: Optional[float] = None): if delta is None: - delta = self.multiverse.acceptable_position_error + delta = self.multiverse.conf.position_tolerance self.assert_list_is_equal(position1, position2, delta=delta) def assert_orientation_is_equal(self, orientation1: List[float], orientation2: List[float], delta: Optional[float] = None): if delta is None: - delta = self.multiverse.acceptable_orientation_error + delta = self.multiverse.conf.orientation_tolerance self.assertAlmostEqual(calculate_angle_between_quaternions(orientation1, orientation2), 0, delta=delta) def assert_list_is_equal(self, list1: List, list2: List, delta: float): From 3bb7ce98faf8eb5306b23b08a4e658c16b07ad0f Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 17 Sep 2024 13:48:36 +0200 Subject: [PATCH 178/189] [WorldObject] Fix issue related to repeated object ID when removing and then adding an object. Add the object name in the comparison if two objects are equal and in the hashing. --- src/pycram/world_concepts/world_object.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index dd6b29ec4..d48d698fd 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -1386,7 +1386,8 @@ def copy_to_world(self, world: World) -> Object: return obj def __eq__(self, other): - return (self.id == other.id and self.world == other.world) if isinstance(other, Object) else False + return (isinstance(other, Object) and self.id == other.id and self.name == other.name + and self.world == other.world) def __hash__(self): - return hash((self.id, self.world)) + return hash((self.id, self.name, self.world)) From e8188ee5eaf8a001041392a1df13c0529717c9c1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 17 Sep 2024 14:54:08 +0200 Subject: [PATCH 179/189] [MultiverseDev] config files moved to root of package and a symlink is now used instead. --- config/multiverse_conf.py | 12 ++++++------ config/world_conf.py | 10 +++++----- demos/pycram_multiverse_demo/demo.py | 2 +- src/pycram/datastructures/world.py | 6 +++--- src/pycram/world_concepts/world_object.py | 4 ++-- src/pycram/world_reasoning.py | 8 ++++---- .../multiverse_communication/client_manager.py | 4 ++-- .../worlds/multiverse_communication/clients.py | 2 +- src/pycram/worlds/multiverse_communication/socket.py | 6 +++--- test/test_cache_manager.py | 4 ++-- test/test_multiverse.py | 2 +- 11 files changed, 30 insertions(+), 30 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index 65a7912a2..dda8905dd 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -10,7 +10,7 @@ class MultiverseConfig(WorldConfig): # Multiverse Configuration - WorldConfig.resources_path = find_multiverse_resources_path() + resources_path = find_multiverse_resources_path() """ The path to the Multiverse resources directory. """ @@ -60,10 +60,10 @@ class MultiverseConfig(WorldConfig): Whether to use the physics simulator state when restoring or saving the world state. """ - WorldConfig.clear_cache_at_start = False + clear_cache_at_start = False - WorldConfig.let_pycram_move_attached_objects = False - WorldConfig.let_pycram_handle_spawning = False + let_pycram_move_attached_objects = False + let_pycram_handle_spawning = False - WorldConfig.position_tolerance = 2e-2 - WorldConfig.prismatic_joint_position_tolerance = 2e-2 + position_tolerance = 2e-2 + prismatic_joint_position_tolerance = 2e-2 diff --git a/config/world_conf.py b/config/world_conf.py index 280cc90dd..c43b07a61 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -9,7 +9,8 @@ class WorldConfig: - resources_path = os.path.join(os.path.dirname(__file__), '..', 'resources') + resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') + resources_path = os.path.abspath(resources_path) """ Global reference for the resources path, this is used to search for the description files of the robot and the objects. @@ -83,7 +84,6 @@ class WorldConfig: """ Whether to raise an error if the goals are not achieved. """ - - @property - def pose_tolerance(self) -> Tuple[float, float]: - return self.position_tolerance, self.orientation_tolerance + @classmethod + def get_pose_tolerance(cls) -> Tuple[float, float]: + return cls.position_tolerance, cls.orientation_tolerance diff --git a/demos/pycram_multiverse_demo/demo.py b/demos/pycram_multiverse_demo/demo.py index abf9cadda..cdc625dac 100644 --- a/demos/pycram_multiverse_demo/demo.py +++ b/demos/pycram_multiverse_demo/demo.py @@ -22,7 +22,7 @@ def move_and_detect(obj_type: ObjectType, pick_pose: Pose): return object_desig -world = Multiverse(simulation='pycram_test') +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])) apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 37890a00b..2619a035b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -95,7 +95,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): self.cache_manager.clear_cache() - GoalValidator.raise_error = World.conf.raise_goal_validator_error + GoalValidator.raise_error = self.conf.raise_goal_validator_error World.simulation_frequency = simulation_frequency if World.current_world is None: @@ -187,11 +187,11 @@ def _init_goal_validators(self): """ # Objects Pose goal validators - self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.pose_tolerance, + self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.get_pose_tolerance(), self.conf.acceptable_percentage_of_goal) self.multi_pose_goal_validator = MultiPoseGoalValidator( lambda x: list(self.get_multiple_object_poses(x).values()), - self.conf.pose_tolerance, self.conf.acceptable_percentage_of_goal) + self.conf.get_pose_tolerance(), self.conf.acceptable_percentage_of_goal) # Joint Goal validators self.joint_position_goal_validator = JointPositionGoalValidator( diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index d48d698fd..0c0246294 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -792,7 +792,7 @@ def current_state(self) -> ObjectState: The current state of this object as an ObjectState. """ return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(), - self.joint_states.copy(), self.world.conf.pose_tolerance) + self.joint_states.copy(), self.world.conf.get_pose_tolerance()) @current_state.setter def current_state(self, state: ObjectState) -> None: @@ -959,7 +959,7 @@ def _set_attached_objects_poses(self, already_moved_objects: Optional[List[Objec :param already_moved_objects: A list of Objects that were already moved, these will be excluded to prevent loops in the update. """ - if not World.conf.let_pycram_move_attached_objects: + if not self.world.conf.let_pycram_move_attached_objects: return if already_moved_objects is None: diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 10d3b8245..8cea3d5f8 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -65,8 +65,8 @@ def contact( def get_visible_objects( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - plot_segmentation_mask: bool = World.conf.DEBUG) -> Tuple[np.ndarray, Pose]: - """W + plot_segmentation_mask: bool = False) -> Tuple[np.ndarray, Pose]: + """ Return a segmentation mask of the objects that are visible from the given camera pose and the front facing axis. :param camera_pose: The pose of the camera in world coordinate frame. @@ -97,7 +97,7 @@ def visible( camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, threshold: float = 0.8, - plot_segmentation_mask: bool = World.conf.DEBUG) -> bool: + plot_segmentation_mask: bool = False) -> bool: """ Checks if an object is visible from a given position. This will be achieved by rendering the object alone and counting the visible pixel, then rendering the complete scene and compare the visible pixels with the @@ -141,7 +141,7 @@ def occluding( obj: Object, camera_pose: Pose, front_facing_axis: Optional[List[float]] = None, - plot_segmentation_mask: bool = World.conf.DEBUG) -> List[Object]: + plot_segmentation_mask: bool = False) -> List[Object]: """ Lists all objects which are occluding the given object. This works similar to 'visible'. First the object alone will be rendered and the position of the pixels of the object in the picture will be saved. diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 5b2363a64..6d52f51fb 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -3,11 +3,11 @@ from ...worlds.multiverse_communication.clients import MultiverseWriter, MultiverseAPI, MultiverseClient, \ MultiverseReader, MultiverseController -from ...config import multiverse_conf as conf +from ...config.multiverse_conf import MultiverseConfig as Conf class MultiverseClientManager: - BASE_PORT: int = conf.BASE_CLIENT_PORT + BASE_PORT: int = Conf.BASE_CLIENT_PORT """ The base port of the Multiverse client. """ diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 2740ab160..4e941ff58 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -42,7 +42,7 @@ def __init__(self, name: str, port: int, is_prospection_world: bool = False, class MultiverseReader(MultiverseClient): - MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = conf.READER_MAX_WAIT_TIME_FOR_DATA + MAX_WAIT_TIME_FOR_DATA: datetime.timedelta = Conf.READER_MAX_WAIT_TIME_FOR_DATA """ The maximum wait time for the data in seconds. """ diff --git a/src/pycram/worlds/multiverse_communication/socket.py b/src/pycram/worlds/multiverse_communication/socket.py index 202a7e4ca..9692f3e86 100644 --- a/src/pycram/worlds/multiverse_communication/socket.py +++ b/src/pycram/worlds/multiverse_communication/socket.py @@ -7,7 +7,7 @@ from typing_extensions import Optional, List, Dict, Callable, TypeVar from ...datastructures.dataclasses import MultiverseMetaData -from ...config import multiverse_conf as conf +from ...config.multiverse_conf import MultiverseConfig as Conf T = TypeVar("T") @@ -17,7 +17,7 @@ class MultiverseSocket: def __init__( self, port: str, - host: str = conf.HOST, + host: str = Conf.HOST, meta_data: MultiverseMetaData = MultiverseMetaData(), ) -> None: """ @@ -35,7 +35,7 @@ def __init__( self._meta_data = meta_data self.client_name = self._meta_data.simulation_name self._multiverse_socket = MultiverseClientPybind( - f"{conf.SERVER_HOST}:{conf.SERVER_PORT}" + f"{Conf.SERVER_HOST}:{Conf.SERVER_PORT}" ) self.request_meta_data = { "meta_data": self._meta_data.__dict__, diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 118650c3b..9f208d90f 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -10,9 +10,9 @@ class TestCacheManager(BulletWorldTestCase): def test_generate_description_and_write_to_cache(self): cache_manager = self.world.cache_manager - path = os.path.join(conf.resources_path, "objects/apartment.urdf") + path = os.path.join(self.world.conf.resources_path, "objects/apartment.urdf") extension = Path(path).suffix - cache_path = os.path.join(conf.cache_dir, "apartment.urdf") + cache_path = os.path.join(self.world.conf.cache_dir, "apartment.urdf") apartment = URDFObject(path) apartment.generate_description_from_file(path, "apartment", extension, cache_path) self.assertTrue(cache_manager.is_cached(path, apartment)) diff --git a/test/test_multiverse.py b/test/test_multiverse.py index eb10edb5e..4c1426211 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -383,7 +383,7 @@ def spawn_cup(position: List) -> Object: def assert_poses_are_equal(self, pose1: Pose, pose2: Pose, position_delta: Optional[float] = None, orientation_delta: Optional[float] = None): if position_delta is None: - position_delta = self.multiverse.conf.ErrorTolerance.position + position_delta = self.multiverse.conf.position_tolerance if orientation_delta is None: orientation_delta = self.multiverse.conf.orientation_tolerance self.assert_position_is_equal(pose1.position_as_list(), pose2.position_as_list(), delta=position_delta) From 43c66ff8957bef552ee4f4a369231df14c339d75 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 17 Sep 2024 16:31:57 +0200 Subject: [PATCH 180/189] [MultiverseDev] cleaning, doc, and renaming of move base joints to mobile base joints. --- src/pycram/datastructures/dataclasses.py | 18 ++++++------- src/pycram/datastructures/enums.py | 4 +-- src/pycram/datastructures/world.py | 20 +++++--------- src/pycram/object_descriptors/mjcf.py | 26 +++++++++++++++++++ src/pycram/object_descriptors/urdf.py | 4 +++ src/pycram/robot_description.py | 12 ++++----- .../robot_descriptions/pr2_description.py | 4 +-- .../robot_descriptions/tiago_description.py | 4 +-- src/pycram/validation/goal_validator.py | 2 +- src/pycram/world_concepts/world_object.py | 26 +++++++++---------- src/pycram/worlds/multiverse.py | 4 +-- 11 files changed, 73 insertions(+), 51 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 4106b5869..1403dff56 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -6,7 +6,7 @@ from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING -from .enums import JointType, Shape, VirtualMoveBaseJointName +from .enums import JointType, Shape, VirtualMobileBaseJointName from .pose import Pose, Point from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable @@ -631,24 +631,24 @@ class TextAnnotation: @dataclass -class VirtualMoveBaseJoints: +class VirtualMobileBaseJoints: """ - Dataclass for storing the names, types and axes of the virtual move base joints of a mobile robot. + Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot. """ - translation_x: Optional[str] = VirtualMoveBaseJointName.LINEAR_X.value - translation_y: Optional[str] = VirtualMoveBaseJointName.LINEAR_Y.value - angular_z: Optional[str] = VirtualMoveBaseJointName.ANGULAR_Z.value + translation_x: Optional[str] = VirtualMobileBaseJointName.LINEAR_X.value + translation_y: Optional[str] = VirtualMobileBaseJointName.LINEAR_Y.value + angular_z: Optional[str] = VirtualMobileBaseJointName.ANGULAR_Z.value @property def names(self) -> List[str]: """ - Return the names of the virtual move base joints. + Return the names of the virtual mobile base joints. """ return [self.translation_x, self.translation_y, self.angular_z] def get_types(self) -> Dict[str, JointType]: """ - Return the joint types of the virtual move base joints. + Return the joint types of the virtual mobile base joints. """ return {self.translation_x: JointType.PRISMATIC, self.translation_y: JointType.PRISMATIC, @@ -656,7 +656,7 @@ def get_types(self) -> Dict[str, JointType]: def get_axes(self) -> Dict[str, Point]: """ - Return the axes (i.e. The axis on which the joint moves) of the virtual move base joints. + Return the axes (i.e. The axis on which the joint moves) of the virtual mobile base joints. """ return {self.translation_x: Point(1, 0, 0), self.translation_y: Point(0, 1, 0), diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 84d5ccaad..1c9d2083a 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -164,9 +164,9 @@ class ImageEnum(Enum): CHAIR = 37 -class VirtualMoveBaseJointName(Enum): +class VirtualMobileBaseJointName(Enum): """ - Enum for the joint names of the virtual move base. + Enum for the joint names of the virtual mobile base. """ LINEAR_X = "odom_vel_lin_x_joint" LINEAR_Y = "odom_vel_lin_y_joint" diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 2619a035b..86c906b2c 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -19,7 +19,7 @@ SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, WorldState, ClosestPointsList, - ContactPointsList, VirtualMoveBaseJoints) + ContactPointsList, VirtualMobileBaseJoints) from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform from ..datastructures.world_entity import StateEntity @@ -649,15 +649,15 @@ def robot_virtual_joints_names(self) -> List[str]: """ The names of the virtual joints of the robot. """ - return self.robot_description.virtual_move_base_joints.names + return self.robot_description.virtual_mobile_base_joints.names - def get_robot_move_base_joints(self) -> VirtualMoveBaseJoints: + def get_robot_mobile_base_joints(self) -> VirtualMobileBaseJoints: """ - Get the move base joints of the robot. + Get the mobile base joints of the robot. - :return: The move base joints. + :return: The mobile base joints. """ - return self.robot_description.virtual_move_base_joints + return self.robot_description.virtual_mobile_base_joints @abstractmethod def perform_collision_detection(self) -> None: @@ -1542,14 +1542,6 @@ def __init__(self): self.prev_world: Optional[World] = None # The previous world is saved to restore it after the with block is exited. - @staticmethod - def sync_worlds(): - """ - Synchronizes the state of the prospection world with the main world. - """ - for world_obj, prospection_obj in World.current_world.world_sync.object_mapping.items(): - prospection_obj.current_state = world_obj.current_state - def __enter__(self): """ This method is called when entering the with block, it will set the current world to the prospection world diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index dd1f39259..232c73d91 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -71,14 +71,21 @@ def name(self) -> str: class JointDescription(AbstractJointDescription): + mjcf_type_map = { MJCFJointType.HINGE.value: JointType.REVOLUTE, MJCFJointType.BALL.value: JointType.SPHERICAL, MJCFJointType.SLIDE.value: JointType.PRISMATIC, MJCFJointType.FREE.value: JointType.FLOATING } + """ + A dictionary mapping the MJCF joint types to the PyCRAM joint types. + """ pycram_type_map = {pycram_type: mjcf_type for mjcf_type, pycram_type in mjcf_type_map.items()} + """ + A dictionary mapping the PyCRAM joint types to the MJCF joint types. + """ def __init__(self, mjcf_description: mjcf.Element, is_virtual: Optional[bool] = False): super().__init__(mjcf_description, is_virtual=is_virtual) @@ -163,6 +170,9 @@ def friction(self) -> float: class ObjectFactory(Factory): + """ + Create MJCF object descriptions from mesh files. + """ def __init__(self, object_name: str, file_path: str, config: Configuration, texture_type: str = "png"): super().__init__(file_path, config) @@ -198,6 +208,13 @@ def __init__(self, object_name: str, file_path: str, config: Configuration, text @staticmethod def add_material_with_texture(geom_builder: GeomBuilder, material_name: str, texture_file_path: str): + """ + Add a material with a texture to the geom builder. + + :param geom_builder: The geom builder to add the material to. + :param material_name: The name of the material. + :param texture_file_path: The path to the texture file. + """ material_property = MaterialProperty(diffuse_color=texture_file_path, opacity=None, emissive_color=None, @@ -206,6 +223,11 @@ def add_material_with_texture(geom_builder: GeomBuilder, material_name: str, tex material_property=material_property) def export_to_mjcf(self, output_file_path: str): + """ + Export the object to a MJCF file. + + :param output_file_path: The path to the output file. + """ exporter = MjcfExporter(self, output_file_path) exporter.build() exporter.export(keep_usd=False) @@ -299,6 +321,10 @@ def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, is_virtual: Optional[bool] = False) -> None: + """ + Finds the child link and adds a joint to it in the object description. + for arguments documentation see :meth:`pycram.description.ObjectDescription.add_joint` + """ position: Optional[List[float]] = None quaternion: Optional[List[float]] = None diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index d4bd2dfb9..80fa88cfa 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -213,6 +213,10 @@ def add_joint(self, name: str, child: str, joint_type: JointType, axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None, lower_limit: Optional[float] = None, upper_limit: Optional[float] = None, is_virtual: Optional[bool] = False) -> None: + """ + Add a joint to the object description, could be a virtual joint as well. + For documentation of the parameters, see :meth:`pycram.description.ObjectDescription.add_joint`. + """ if lower_limit is not None or upper_limit is not None: limit = urdf.JointLimit(lower=lower_limit, upper=upper_limit) else: diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 46a7eed4a..dad64f8e7 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -4,7 +4,7 @@ import rospy from typing_extensions import List, Dict, Union, Optional -from .datastructures.dataclasses import VirtualMoveBaseJoints +from .datastructures.dataclasses import VirtualMobileBaseJoints from .datastructures.enums import Arms, Grasp, GripperState, GripperType, JointType from .object_descriptors.urdf import ObjectDescription as URDFObject from .utils import suppress_stdout_stderr @@ -112,14 +112,14 @@ class RobotDescription: """ All joints defined in the URDF, by default fixed joints are not included """ - virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None """ - Virtual move base joint names for mobile robots, these joints are not part of the URDF, however they are used to + Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to move the robot in the simulation (e.g. set_pose for the robot would actually move these joints) """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = None, mjcf_path: Optional[str] = None): + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -129,7 +129,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param torso_link: Torso link of the robot :param torso_joint: Torso joint of the robot, this is the joint that moves the torso upwards if there is one :param urdf_path: Path to the URDF file of the robot - :param virtual_move_base_joints: Virtual move base joint names for mobile robots + :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot """ self.name = name @@ -146,7 +146,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, self.grasps: Dict[Grasp, List[float]] = {} self.links: List[str] = [l.name for l in self.urdf_object.links] self.joints: List[str] = [j.name for j in self.urdf_object.joints] - self.virtual_move_base_joints: Optional[VirtualMoveBaseJoints] = virtual_move_base_joints + self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints @property def has_actuators(self): diff --git a/src/pycram/robot_descriptions/pr2_description.py b/src/pycram/robot_descriptions/pr2_description.py index f87083840..b7fe30ad7 100644 --- a/src/pycram/robot_descriptions/pr2_description.py +++ b/src/pycram/robot_descriptions/pr2_description.py @@ -1,4 +1,4 @@ -from ..datastructures.dataclasses import VirtualMoveBaseJoints +from ..datastructures.dataclasses import VirtualMobileBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription from ..datastructures.enums import Arms, Grasp, GripperState, GripperType @@ -12,7 +12,7 @@ mjcf_filename = get_robot_mjcf_path("", "pr2") pr2_description = RobotDescription("pr2", "base_link", "torso_lift_link", "torso_lift_joint", - filename, virtual_move_base_joints=VirtualMoveBaseJoints(), mjcf_path=mjcf_filename) + filename, virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "torso_lift_link", "l_wrist_roll_link", diff --git a/src/pycram/robot_descriptions/tiago_description.py b/src/pycram/robot_descriptions/tiago_description.py index bff284557..37b1ec29e 100644 --- a/src/pycram/robot_descriptions/tiago_description.py +++ b/src/pycram/robot_descriptions/tiago_description.py @@ -1,6 +1,6 @@ import rospkg -from ..datastructures.dataclasses import VirtualMoveBaseJoints +from ..datastructures.dataclasses import VirtualMobileBaseJoints from ..datastructures.enums import GripperState, Arms, Grasp from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription @@ -13,7 +13,7 @@ tiago_description = RobotDescription("tiago_dual", "base_link", "torso_lift_link", "torso_lift_joint", filename, - virtual_move_base_joints=VirtualMoveBaseJoints(), + virtual_mobile_base_joints=VirtualMobileBaseJoints(), mjcf_path=mjcf_filename) ################################## Left Arm ################################## diff --git a/src/pycram/validation/goal_validator.py b/src/pycram/validation/goal_validator.py index 0b9f7cd17..a70f2b438 100644 --- a/src/pycram/validation/goal_validator.py +++ b/src/pycram/validation/goal_validator.py @@ -529,7 +529,7 @@ def validate_multiple_joint_positions(position_setter_func): A decorator to validate the joint positions, this function does not validate the virtual joints, as in multiverse the virtual joints take command velocities and not positions, so after their goals are set, they are zeroed thus can't be validated. (They are actually validated by the robot pose in case - of virtual move base joints) + of virtual mobile base joints) :param position_setter_func: The function to set the joint positions. """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 0c0246294..e2f76b69d 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -134,27 +134,27 @@ def _resolve_description(self, path: Optional[str] = None, description: Optional def set_mobile_robot_pose(self, pose: Pose) -> None: """ - Set the goal for the move base joints of a mobile robot to reach a target pose. This is used for example when + Set the goal for the mobile base joints of a mobile robot to reach a target pose. This is used for example when the simulator does not support setting the pose of the robot directly (e.g. MuJoCo). :param pose: The target pose. """ - goal = self.get_move_base_joint_goal(pose) + goal = self.get_mobile_base_joint_goal(pose) self.set_multiple_joint_positions(goal) - def get_move_base_joint_goal(self, pose: Pose) -> Dict[str, float]: + def get_mobile_base_joint_goal(self, pose: Pose) -> Dict[str, float]: """ - Get the goal for the move base joints of a mobile robot to reach a target pose. + Get the goal for the mobile base joints of a mobile robot to reach a target pose. :param pose: The target pose. - :return: The goal for the move base joints. + :return: The goal for the mobile base joints. """ target_translation, target_angle = self.get_mobile_base_pose_difference(pose) # Get the joints of the base link - move_base_joints = self.world.get_robot_move_base_joints() - return {move_base_joints.translation_x: target_translation.x, - move_base_joints.translation_y: target_translation.y, - move_base_joints.angular_z: target_angle} + mobile_base_joints = self.world.get_robot_mobile_base_joints() + return {mobile_base_joints.translation_x: target_translation.x, + mobile_base_joints.translation_y: target_translation.y, + mobile_base_joints.angular_z: target_angle} def get_mobile_base_pose_difference(self, pose: Pose) -> Tuple[Point, float]: """ @@ -319,7 +319,7 @@ def _spawn_object_and_get_id(self) -> int: def _update_world_robot_and_description(self): """ Initialize the robot description of the object, load the description from the RobotDescriptionManager and set - the robot as the current robot in the World. Also add the virtual move base joints to the robot. + the robot as the current robot in the World. Also add the virtual mobile base joints to the robot. """ rdm = RobotDescriptionManager() rdm.load_description(self.description.name) @@ -328,9 +328,9 @@ def _update_world_robot_and_description(self): def _add_virtual_move_base_joints(self): """ - Add the virtual move base joints to the robot description. + Add the virtual mobile base joints to the robot description. """ - virtual_joints = self.robot_description.virtual_move_base_joints + virtual_joints = self.robot_description.virtual_mobile_base_joints if virtual_joints is None: return child_link = self.description.get_root() @@ -925,7 +925,7 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: joint.current_state = joint_states[joint.id] def robot_virtual_move_base_joints_names(self): - return self.robot_description.virtual_move_base_joints.names + return self.robot_description.virtual_mobile_base_joints.names def remove_saved_states(self) -> None: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 3e8604293..8e4357064 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -375,7 +375,7 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: return False if (obj.obj_type == ObjectType.ROBOT and - RobotDescription.current_robot_description.virtual_move_base_joints is not None): + RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): obj.set_mobile_robot_pose(pose) else: self._set_body_pose(obj.name, pose) @@ -391,7 +391,7 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> None """ for obj in objects.keys(): if (obj.obj_type == ObjectType.ROBOT and - RobotDescription.current_robot_description.virtual_move_base_joints is not None): + RobotDescription.current_robot_description.virtual_mobile_base_joints is not None): obj.set_mobile_robot_pose(objects[obj]) objects = {obj: pose for obj, pose in objects.items() if obj.obj_type not in [ObjectType.ENVIRONMENT, ObjectType.ROBOT]} From 8443fc5c37d8f2bb0f8388bac98fb394e33a3237 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 17 Sep 2024 16:56:28 +0200 Subject: [PATCH 181/189] [MultiverseDev] added deprecated library for using deprecated decorators on old deprecated code. --- requirements.txt | 3 ++- src/pycram/robot_descriptions/__init__.py | 3 ++- src/pycram/world_concepts/world_object.py | 18 ++++++++---------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/requirements.txt b/requirements.txt index 23355f6f1..e88f57438 100644 --- a/requirements.txt +++ b/requirements.txt @@ -25,4 +25,5 @@ playsound~=1.3.0 pydub~=0.25.1 gTTS~=2.5.3 dm_control -trimesh \ No newline at end of file +trimesh +deprecated \ No newline at end of file diff --git a/src/pycram/robot_descriptions/__init__.py b/src/pycram/robot_descriptions/__init__.py index 33dc54ca9..1ec759998 100644 --- a/src/pycram/robot_descriptions/__init__.py +++ b/src/pycram/robot_descriptions/__init__.py @@ -9,7 +9,8 @@ class DeprecatedRobotDescription: def raise_error(self): - raise DeprecationWarning("Robot description moved, please use RobotDescription.current_robot_description from pycram.robot_description") + raise DeprecationWarning("Robot description moved, please use RobotDescription.current_robot_description from" + " pycram.robot_description") @property def name(self): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index e2f76b69d..5a463d965 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -8,6 +8,7 @@ import rospy from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union +from deprecated import deprecated from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, @@ -224,14 +225,6 @@ def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: """ return self.world.get_multiple_link_poses(links) - def get_target_poses_of_attached_objects(self) -> Dict[Object, Pose]: - """ - Get the target poses of the attached objects. - - :return: The target poses of the attached objects - """ - return self.get_target_poses_of_attached_objects_given_parent(self.get_pose()) - def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: """ Get the poses of the attached objects. @@ -243,9 +236,10 @@ def get_poses_of_attached_objects(self) -> Dict[Object, Pose]: def get_target_poses_of_attached_objects_given_parent(self, pose: Pose) -> Dict[Object, Pose]: """ - Get the target poses of the attached objects of an object. Given the pose of the parent object. - param pose: The pose of the parent object. + Get the target poses of the attached objects of an object. Given the pose of the parent object. (i.e. the poses + to which the attached objects will move when the parent object is at the given pose) + :param pose: The pose of the parent object. :return: The target poses of the attached objects """ return {child_object: attachment.get_child_object_pose_given_parent(pose) for child_object, attachment @@ -1112,6 +1106,10 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None: if self.world.reset_joint_position(self.joints[joint_name], joint_position): self._update_on_joint_position_change() + @deprecated("Use set_multiple_joint_positions instead") + def set_joint_positions(self, joint_positions: Dict[str, float]) -> None: + self.set_multiple_joint_positions(joint_positions) + def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> None: """ Set the current position of multiple joints at once, this method should be preferred when setting From 05c857478a6a29421048bb6f51fd17c860fc3827 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 18 Sep 2024 17:34:17 +0200 Subject: [PATCH 182/189] [MultiverseDev] cleaning, and doc. --- src/pycram/helper.py | 2 + src/pycram/utils.py | 16 +++++++ src/pycram/worlds/bullet_world.py | 10 +++-- src/pycram/worlds/multiverse.py | 43 ++++++++----------- .../multiverse_communication/clients.py | 16 ++----- 5 files changed, 47 insertions(+), 40 deletions(-) diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 015062651..4f67e2c4a 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -111,3 +111,5 @@ def find_multiverse_path() -> Optional[str]: return multiverse_path + 'multiverse' return None + + diff --git a/src/pycram/utils.py b/src/pycram/utils.py index eb0b6fb28..30bd10538 100644 --- a/src/pycram/utils.py +++ b/src/pycram/utils.py @@ -392,3 +392,19 @@ def plot_depth_image(depth_image): cbar.set_label('Depth Value') plt.show() + + +def wxyz_to_xyzw(wxyz: List[float]) -> List[float]: + """ + Convert a quaternion from WXYZ to XYZW format. + """ + return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] + + +def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: + """ + Convert a quaternion from XYZW to WXYZ format. + + :param xyzw: The quaternion in XYZW format. + """ + return [xyzw[3], *xyzw[:3]] \ No newline at end of file diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index e6985e60a..9bcc01ca9 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -177,9 +177,11 @@ def perform_collision_detection(self) -> None: def get_object_contact_points(self, obj: Object) -> ContactPointsList: """ - For a more detailed explanation of the - returned list please look at: - `PyBullet Doc `_ + Get the contact points of the object with akk other objects in the world. The contact points are returned as a + ContactPointsList object. + + :param obj: The object for which the contact points should be returned. + :return: The contact points of the object with all other objects in the world. """ self.perform_collision_detection() points_list = p.getContactPoints(obj.id, physicsClientId=self.id) @@ -201,6 +203,8 @@ def parse_points_list_to_args(self, point: List) -> Dict: """ Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the ContactPoint class. + + :param point: The list of points. """ return {"link_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]), "link_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]), diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 8e4357064..4cda8cedc 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -17,7 +17,7 @@ from ..description import Link, Joint, ObjectDescription from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescription -from ..utils import RayTestUtils +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 from ..world_concepts.constraints import Constraint @@ -191,7 +191,7 @@ def spawn_robot_with_controller(self, name: str, pose: Pose) -> None: } self.joint_controller.init_controller(actuator_joint_commands) self.writer.spawn_robot_with_actuators(name, pose.position_as_list(), - self.xyzw_to_wxyz(pose.orientation_as_list()), + xyzw_to_wxyz(pose.orientation_as_list()), actuator_joint_commands) def load_object_and_get_id(self, name: Optional[str] = None, @@ -284,6 +284,10 @@ def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> b """ Set the positions of multiple joints in the simulator. Also check if the joint is controlled by an actuator and use the controller to set the joint position if the joint is controlled. + + :param joint_positions: The dictionary of joints and positions. + :return: True if the joint positions are set successfully (this means that the joint positions are set without + errors, but not necessarily that the joint positions are set to the specified values). """ if self.conf.use_controller: @@ -303,6 +307,7 @@ def get_controlled_joints(self, joints: Optional[List[Joint]] = None) -> List[Jo Get the joints that are controlled by an actuator from the list of joints. :param joints: The list of joints to check. + :return: The list of controlled joints. """ joints = self.robot.joints if joints is None else joints return [joint for joint in joints if self.joint_has_actuator(joint)] @@ -358,15 +363,21 @@ def get_multiple_link_poses(self, links: List[Link]) -> Dict[str, Pose]: return self._get_multiple_body_poses([link.name for link in links]) def get_object_pose(self, obj: Object) -> Pose: - if obj.obj_type == ObjectType.ENVIRONMENT: + if obj.has_type_environment(): return Pose() return self._get_body_pose(obj.name) def get_multiple_object_poses(self, objects: List[Object]) -> Dict[str, Pose]: - env_objects = [obj for obj in objects if obj.obj_type == ObjectType.ENVIRONMENT] - non_env_objects = [obj for obj in objects if obj.obj_type != ObjectType.ENVIRONMENT] + """ + Set the poses of multiple objects in the simulator. If the object is of type environment, the pose will be + the default pose. + + :param objects: The list of objects. + :return: The dictionary of object names and poses. + """ + non_env_objects = [obj for obj in objects if not obj.has_type_environment()] all_poses = self._get_multiple_body_poses([obj.name for obj in non_env_objects]) - all_poses.update({obj.name: Pose() for obj in env_objects}) + all_poses.update({obj.name: Pose() for obj in objects if obj.has_type_environment()}) return all_poses @validate_object_pose @@ -414,7 +425,7 @@ def _set_multiple_body_poses(self, body_poses: Dict[str, Pose]) -> None: """ self.writer.set_multiple_body_poses({name: {MultiverseBodyProperty.POSITION: pose.position_as_list(), MultiverseBodyProperty.ORIENTATION: - self.xyzw_to_wxyz(pose.orientation_as_list()), + xyzw_to_wxyz(pose.orientation_as_list()), MultiverseBodyProperty.RELATIVE_VELOCITY: [0.0] * 6} for name, pose in body_poses.items()}) @@ -428,14 +439,7 @@ def _get_body_pose(self, body_name: str, wait: Optional[bool] = True) -> Optiona """ data = self.reader.get_body_pose(body_name, wait) return Pose(data[MultiverseBodyProperty.POSITION.value], - self.wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) - - @staticmethod - def wxyz_to_xyzw(wxyz: List[float]) -> List[float]: - """ - Convert a quaternion from WXYZ to XYZW format. - """ - return [wxyz[1], wxyz[2], wxyz[3], wxyz[0]] + wxyz_to_xyzw(data[MultiverseBodyProperty.ORIENTATION.value])) def _get_multiple_body_poses(self, body_names: List[str]) -> Dict[str, Pose]: """ @@ -463,15 +467,6 @@ def multiverse_reset_world(self): """ self.writer.reset_world() - @staticmethod - def xyzw_to_wxyz(xyzw: List[float]) -> List[float]: - """ - Convert a quaternion from XYZW to WXYZ format. - - :param xyzw: The quaternion in XYZW format. - """ - return [xyzw[3], *xyzw[:3]] - def disconnect_from_physics_server(self) -> None: MultiverseClientManager.stop_all_clients() diff --git a/src/pycram/worlds/multiverse_communication/clients.py b/src/pycram/worlds/multiverse_communication/clients.py index 4e941ff58..d98983ab1 100644 --- a/src/pycram/worlds/multiverse_communication/clients.py +++ b/src/pycram/worlds/multiverse_communication/clients.py @@ -13,7 +13,7 @@ from ...datastructures.enums import (MultiverseAPIName as API, MultiverseBodyProperty as BodyProperty, MultiverseProperty as Property) from ...datastructures.pose import Pose -from ...datastructures.world import World +from ...utils import wxyz_to_xyzw from ...world_concepts.constraints import Constraint from ...world_concepts.world_object import Object, Link @@ -92,7 +92,7 @@ def get_multiple_body_poses(self, body_names: List[str], wait: bool = False) -> wait=wait) if data is not None: return {name: Pose(data[name][BodyProperty.POSITION.value], - self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) + wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value])) for name in body_names} def get_body_position(self, name: str, wait: bool = False) -> Optional[List[float]]: @@ -137,7 +137,7 @@ def get_multiple_body_orientations(self, body_names: List[str], """ data = self.get_multiple_body_properties(body_names, [BodyProperty.ORIENTATION], wait=wait) if data is not None: - return {name: self.wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} + return {name: wxyz_to_xyzw(data[name][BodyProperty.ORIENTATION.value]) for name in body_names} def get_body_property(self, name: str, property_: Property, wait: bool = False) -> Optional[List[float]]: """ @@ -164,16 +164,6 @@ def get_multiple_body_properties(self, body_names: List[str], properties: List[P """ return self.get_multiple_body_data(body_names, {name: properties for name in body_names}, wait=wait) - @staticmethod - def wxyz_to_xyzw(quaternion: List[float]) -> List[float]: - """ - Convert the quaternion from wxyz to xyzw. - - :param quaternion: The quaternion as a list of floats. - :return: The quaternion as a list of floats. - """ - return quaternion[1:] + [quaternion[0]] - def get_body_data(self, name: str, properties: Optional[List[Property]] = None, wait: bool = False) -> Optional[Dict]: From 066bd8bbb86b40a3b114c415b66432b1ca9826c9 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 18 Sep 2024 17:52:40 +0200 Subject: [PATCH 183/189] [MultiverseDev] merge with dev. --- src/pycram/world_concepts/world_object.py | 5 +++-- src/pycram/worlds/bullet_world.py | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 0cab78b47..147e6e906 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -6,11 +6,10 @@ import numpy as np import rospy +from deprecated import deprecated from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union -from deprecated import deprecated -from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, ContactPointsList) @@ -22,7 +21,9 @@ from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \ ObjectDescriptionUndefined from ..local_transformer import LocalTransformer +from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription from ..object_descriptors.urdf import ObjectDescription as URDF + try: from ..object_descriptors.mjcf import ObjectDescription as MJCF except ImportError: diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index f6a4ffc29..36443ebff 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -312,7 +312,7 @@ def remove_physics_simulator_state(self, state_id: int): p.removeState(state_id, physicsClientId=self.id) def _add_vis_axis(self, pose: Pose, - length: Optional[float] = 0.2) -> int: + length: Optional[float] = 0.2) -> int: """ Creates a Visual object which represents the coordinate frame at the given position and orientation. There can be an unlimited amount of vis axis objects. @@ -402,8 +402,8 @@ def get_images_for_target(self, physicsClientId=self.id))[2:5] def _add_text(self, text: str, position: List[float], orientation: Optional[List[float]] = None, - size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, - parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: + size: Optional[float] = None, color: Optional[Color] = Color(), life_time: Optional[float] = 0, + parent_object_id: Optional[int] = None, parent_link_id: Optional[int] = None) -> int: args = {} if orientation: args["textOrientation"] = orientation From e6c0881b357a9382a453fe8b24bbf396356390c7 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 18 Sep 2024 17:55:04 +0200 Subject: [PATCH 184/189] [MultiverseDev] added config symlink --- src/pycram/config | 1 + 1 file changed, 1 insertion(+) create mode 120000 src/pycram/config diff --git a/src/pycram/config b/src/pycram/config new file mode 120000 index 000000000..899f69898 --- /dev/null +++ b/src/pycram/config @@ -0,0 +1 @@ +../../config \ No newline at end of file From 8bc8461cf553acb064ff9bb18b45a41c842d1924 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 20 Sep 2024 18:25:37 +0200 Subject: [PATCH 185/189] [MultiverseDev] Doc and Cleaning, fixing requested changes. --- config/multiverse_conf.py | 11 ++-- config/world_conf.py | 11 ++-- src/pycram/__init__.py | 4 +- src/pycram/cache_manager.py | 2 +- src/pycram/costmaps.py | 7 +-- src/pycram/datastructures/dataclasses.py | 43 ++++++------- src/pycram/datastructures/enums.py | 4 +- src/pycram/datastructures/pose.py | 21 ++----- src/pycram/datastructures/world.py | 35 ++++++----- src/pycram/datastructures/world_entity.py | 4 +- src/pycram/description.py | 61 ++++--------------- src/pycram/designator.py | 11 ++-- src/pycram/external_interfaces/giskard.py | 8 +-- src/pycram/helper.py | 8 +-- src/pycram/local_transformer.py | 8 +-- src/pycram/object_descriptors/mjcf.py | 6 +- src/pycram/object_descriptors/urdf.py | 5 +- src/pycram/process_module.py | 26 ++++---- src/pycram/robot_description.py | 28 ++++----- src/pycram/world_concepts/constraints.py | 50 ++++----------- src/pycram/world_reasoning.py | 2 +- src/pycram/worlds/multiverse.py | 6 +- .../client_manager.py | 2 +- 23 files changed, 142 insertions(+), 221 deletions(-) diff --git a/config/multiverse_conf.py b/config/multiverse_conf.py index dda8905dd..6463cf151 100644 --- a/config/multiverse_conf.py +++ b/config/multiverse_conf.py @@ -18,7 +18,7 @@ class MultiverseConfig(WorldConfig): # Multiverse Socket Configuration HOST: str = "tcp://127.0.0.1" SERVER_HOST: str = HOST - SERVER_PORT: str = "7000" + SERVER_PORT: str = 7000 BASE_CLIENT_PORT: int = 9000 # Multiverse Client Configuration @@ -30,6 +30,9 @@ class MultiverseConfig(WorldConfig): # Multiverse Simulation Configuration simulation_time_step: datetime.timedelta = datetime.timedelta(milliseconds=10) simulation_frequency: int = int(1 / simulation_time_step.total_seconds()) + """ + The time step of the simulation in seconds and the frequency of the simulation in Hz. + """ simulation_wait_time_factor: float = 1.0 """ @@ -38,16 +41,16 @@ class MultiverseConfig(WorldConfig): running the simulation. """ - use_bullet_mode: bool = True + use_static_mode: bool = True """ If True, the simulation will always be in paused state unless the simulate() function is called, this behaves similar to bullet_world which uses the bullet physics engine. """ use_controller: bool = False - use_controller = use_controller and not use_bullet_mode + use_controller = use_controller and not use_static_mode """ - Only used when use_bullet_mode is False. This turns on the controller for the robot joints. + Only used when use_static_mode is False. This turns on the controller for the robot joints. """ default_description_type: Type[ObjectDescription] = MJCF diff --git a/config/world_conf.py b/config/world_conf.py index c43b07a61..7d11942c9 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -1,6 +1,5 @@ import math import os -from dataclasses import dataclass from typing_extensions import Tuple, Type from pycram.description import ObjectDescription @@ -9,6 +8,11 @@ class WorldConfig: + """ + A class to store the configuration of the world, this can be inherited to create a new configuration class for a + specific world (e.g. multiverse has MultiverseConfig which inherits from this class). + """ + resources_path = os.path.join(os.path.dirname(__file__), '..', '..', '..', 'resources') resources_path = os.path.abspath(resources_path) """ @@ -41,11 +45,6 @@ class WorldConfig: Whether to update the poses from the simulator when getting the object poses. """ - DEBUG: bool = False - """ - Whether to use in debug mode. (This is used to print debug messages, plot images, etc.) - """ - default_description_type: Type[ObjectDescription] = URDF """ The default description type for the objects. diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index a8c27ca5f..9fde2cdac 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -1,5 +1,5 @@ -import pycram.process_modules -import pycram.robot_descriptions +from . import process_modules +from . import robot_descriptions # from .specialized_designators import * from .datastructures.world import World diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index ca829fc72..3e6a9889d 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -17,7 +17,7 @@ class CacheManager: cache_cleared: bool = False """ - A class variable that indicates whether the cache directory has been cleared. + Indicate whether the cache directory has been cleared at least once since beginning or not. """ def __init__(self, cache_dir: str, data_directory: List[str], clear_cache: bool = True): diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index 87c2de0f3..ad34677e6 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -774,11 +774,10 @@ def generate_map(self) -> None: def get_aabb_for_link(self) -> AxisAlignedBoundingBox: """ - Returns the axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let the - AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the - identity orientation. - :return: Two points in world coordinate space, which span a rectangle + :return: The axis aligned bounding box (AABB) of the link provided when creating this costmap. To try and let + the AABB as close to the actual object as possible, the Object will be rotated such that the link will be in the + identity orientation. """ prospection_object = World.current_world.get_prospection_object_for_object(self.object) with UseProspectionWorld(): diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 1403dff56..040189ce3 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -111,48 +111,36 @@ def from_min_max(cls, min_point: List[float], max_point: List[float]): def get_min_max_points(self) -> Tuple[Point, Point]: """ - Return the axis-aligned bounding box as a tuple of minimum and maximum points. - :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ return self.get_min_point(), self.get_max_point() def get_min_point(self) -> Point: """ - Return the axis-aligned bounding box as a minimum point. - :return: The axis-aligned bounding box as a minimum point """ return Point(self.min_x, self.min_y, self.min_z) def get_max_point(self) -> Point: """ - Return the axis-aligned bounding box as a maximum point. - :return: The axis-aligned bounding box as a maximum point """ return Point(self.max_x, self.max_y, self.max_z) def get_min_max(self) -> Tuple[List[float], List[float]]: """ - Return the axis-aligned bounding box as a tuple of minimum and maximum points. - :return: The axis-aligned bounding box as a tuple of minimum and maximum points """ return self.get_min(), self.get_max() def get_min(self) -> List[float]: """ - Returns the minimum point of the axis-aligned bounding box. - :return: The minimum point of the axis-aligned bounding box """ return [self.min_x, self.min_y, self.min_z] def get_max(self) -> List[float]: """ - Returns the maximum point of the axis-aligned bounding box. - :return: The maximum point of the axis-aligned bounding box """ return [self.max_x, self.max_y, self.max_z] @@ -161,7 +149,8 @@ def get_max(self) -> List[float]: @dataclass class CollisionCallbacks: """ - Dataclass for storing the collision callbacks. + Dataclass for storing the collision callbacks which are callables that get called when there is a collision + or when a collision is no longer there. """ on_collision_cb: Callable no_collision_cb: Optional[Callable] = None @@ -195,7 +184,7 @@ class VisualShape(ABC): @abstractmethod def shape_data(self) -> Dict[str, Any]: """ - Returns the shape data of the visual shape (e.g. half extents for a box, radius for a sphere) as a dictionary. + :return: the shape data of the visual shape (e.g. half extents for a box, radius for a sphere) as a dictionary. """ pass @@ -203,7 +192,7 @@ def shape_data(self) -> Dict[str, Any]: @abstractmethod def visual_geometry_type(self) -> Shape: """ - Returns the visual geometry type of the visual shape (e.g. box, sphere) as a Shape object. + :return: The visual geometry type of the visual shape (e.g. box, sphere) as a Shape object. """ pass @@ -318,20 +307,22 @@ class LinkState(State): def __eq__(self, other: 'LinkState'): return self.all_constraints_exist(other) and self.all_constraints_are_equal(other) - def all_constraints_exist(self, other: 'LinkState'): + def all_constraints_exist(self, other: 'LinkState') -> bool: """ Check if all constraints exist in the other link state. :param other: The state of the other link. + :return: True if all constraints exist, False otherwise. """ return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()]) and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys())) - def all_constraints_are_equal(self, other: 'LinkState'): + def all_constraints_are_equal(self, other: 'LinkState') -> bool: """ Check if all constraints are equal to the ones in the other link state. :param other: The state of the other link. + :return: True if all constraints are equal, False otherwise. """ return all([cid == other_cid for cid, other_cid in zip(self.constraint_ids.values(), other.constraint_ids.values())]) @@ -373,28 +364,31 @@ def __eq__(self, other: 'ObjectState'): and self.link_states == other.link_states and self.joint_states == other.joint_states) - def pose_is_almost_equal(self, other: 'ObjectState'): + def pose_is_almost_equal(self, other: 'ObjectState') -> bool: """ Check if the pose of the object is almost equal to the pose of another object. :param other: The state of the other object. + :return: True if the poses are almost equal, False otherwise. """ return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1]) - def all_attachments_exist(self, other: 'ObjectState'): + def all_attachments_exist(self, other: 'ObjectState') -> bool: """ Check if all attachments exist in the other object state. :param other: The state of the other object. + :return: True if all attachments exist, False otherwise. """ return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()]) and len(self.attachments.keys()) == len(other.attachments.keys())) - def all_attachments_are_equal(self, other: 'ObjectState'): + def all_attachments_are_equal(self, other: 'ObjectState') -> bool: """ Check if all attachments are equal to the ones in the other object state. :param other: The state of the other object. + :return: True if all attachments are equal, False otherwise """ return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())]) @@ -417,28 +411,31 @@ 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)) - def simulator_state_is_equal(self, other: 'WorldState'): + def simulator_state_is_equal(self, other: 'WorldState') -> bool: """ Check if the simulator state is equal to the simulator state of another world state. :param other: The state of the other world. + :return: True if the simulator states are equal, False otherwise. """ return self.simulator_state_id == other.simulator_state_id - def all_objects_exist(self, other: 'WorldState'): + def all_objects_exist(self, other: 'WorldState') -> bool: """ Check if all objects exist in the other world state. :param other: The state of the other world. + :return: True if all objects exist, False otherwise. """ return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()]) and len(self.object_states.keys()) == len(other.object_states.keys())) - def all_objects_states_are_equal(self, other: 'WorldState'): + def all_objects_states_are_equal(self, other: 'WorldState') -> bool: """ Check if all object states are equal to the ones in the other world state. :param other: The state of the other world. + :return: True if all object states are equal, False otherwise. """ return all([obj_state == other_obj_state for obj_state, other_obj_state in zip(self.object_states.values(), diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 223ec26aa..92968ba61 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -247,7 +247,7 @@ class MultiverseJointPosition(MultiverseJointProperty): PRISMATIC_JOINT_POSITION = "joint_tvalue" @classmethod - def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointPosition': + def from_pycram_joint_type(cls, joint_type: JointType) -> 'MultiverseJointPosition': if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: return MultiverseJointPosition.REVOLUTE_JOINT_POSITION elif joint_type == JointType.PRISMATIC: @@ -264,7 +264,7 @@ class MultiverseJointCMD(MultiverseJointProperty): PRISMATIC_JOINT_CMD = "cmd_joint_tvalue" @classmethod - def from_pycram_joint_type(cls, joint_type: 'JointType') -> 'MultiverseJointCMD': + def from_pycram_joint_type(cls, joint_type: JointType) -> 'MultiverseJointCMD': if joint_type in [JointType.REVOLUTE, JointType.CONTINUOUS]: return MultiverseJointCMD.REVOLUTE_JOINT_CMD elif joint_type == JointType.PRISMATIC: diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 1ca3a228f..490a56e9f 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -5,7 +5,7 @@ import datetime from tf.transformations import euler_from_quaternion -from typing_extensions import List, Union, Optional, Sized +from typing_extensions import List, Union, Optional, Sized, Self import numpy as np import rospy @@ -88,7 +88,7 @@ def from_pose_stamped(pose_stamped: PoseStamped) -> Pose: p.pose = pose_stamped.pose return p - def get_position_diff(self, target_pose: 'Pose') -> Point: + def get_position_diff(self, target_pose: Self) -> Point: """ Get the difference between the target and the current positions. @@ -98,7 +98,7 @@ def get_position_diff(self, target_pose: 'Pose') -> Point: return Point(target_pose.position.x - self.position.x, target_pose.position.y - self.position.y, target_pose.position.z - self.position.z) - def get_z_angle_difference(self, target_pose: Pose) -> float: + def get_z_angle_difference(self, target_pose: Self) -> float: """ Get the difference between two z angles. @@ -189,8 +189,6 @@ def orientation(self, value) -> None: def to_list(self) -> List[List[float]]: """ - Returns the position and orientation of this pose as a list containing two list. - :return: The position and orientation as lists """ return [[self.pose.position.x, self.pose.position.y, self.pose.position.z], @@ -218,16 +216,12 @@ def copy(self) -> Pose: def position_as_list(self) -> List[float]: """ - Returns only the position as a list of xyz. - - :return: The position as a list + :return: The position as a list of xyz values. """ return [self.position.x, self.position.y, self.position.z] def orientation_as_list(self) -> List[float]: """ - Returns only the orientation as a list of a quaternion - :return: The orientation as a quaternion with xyzw """ return [self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w] @@ -390,9 +384,6 @@ def apply_transform_to_array_of_points(self, points: np.ndarray) -> np.ndarray: def get_homogeneous_matrix(self) -> np.ndarray: """ - Returns the homogeneous matrix of this Transform. The matrix can be used to transform points from the frame_id - to the child_frame_id. - :return: The homogeneous matrix of this Transform """ translation = transformations.translation_matrix(self.translation_as_list()) @@ -502,16 +493,12 @@ def copy(self) -> Transform: def translation_as_list(self) -> List[float]: """ - Returns the translation of this Transform as a list. - :return: The translation as a list of xyz """ return [self.transform.translation.x, self.transform.translation.y, self.transform.translation.z] def rotation_as_list(self) -> List[float]: """ - Returns the rotation of this Transform as a list representing a quaternion. - :return: The rotation of this Transform as a list with xyzw """ return [self.transform.rotation.x, self.transform.rotation.y, self.transform.rotation.z, diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 86c906b2c..a152a4938 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -717,8 +717,11 @@ def get_closest_points_between_objects(self, object_a: Object, object_b: Object, def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: """ Reset the joint position instantly without physics simulation - NOTE: It is recommended to use the validate_joint_position decorator to validate the joint position for - the implementation of this method. + + .. note:: + It is recommended to use the validate_joint_position decorator to validate the joint position for + the implementation of this method. + :param joint: The joint to reset the position for. :param joint_position: The new joint pose. :return: True if the reset was successful, False otherwise @@ -730,8 +733,10 @@ def reset_joint_position(self, joint: Joint, joint_position: float) -> bool: def set_multiple_joint_positions(self, joint_positions: Dict[Joint, float]) -> bool: """ Set the positions of multiple joints of an articulated object. - NOTE: It is recommended to use the validate_multiple_joint_positions decorator to validate the - joint positions for the implementation of this method. + + .. note:: + It is recommended to use the validate_multiple_joint_positions decorator to validate the + joint positions for the implementation of this method. :param joint_positions: A dictionary with joint objects as keys and joint positions as values. :return: True if the set was successful, False otherwise. @@ -753,8 +758,10 @@ def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: """ Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation. - NOTE: It is recommended to use the validate_object_pose decorator to validate the object pose for the - implementation of this method. + + .. note:: + It is recommended to use the validate_object_pose decorator to validate the object pose for the + implementation of this method. :param obj: The object. :param pose: The new pose as a Pose object. @@ -1423,7 +1430,7 @@ def disable_joint_force_torque_sensor(self, obj: Object, joint_id: int) -> None: def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[float]: """ - Returns the joint reaction forces and torques of the specified joint. + Get the joint reaction forces and torques of the specified joint. :param obj: The object in which the joint is located. :param joint_id: The id of the joint for which the force torque should be returned. @@ -1433,7 +1440,7 @@ def get_joint_reaction_force_torque(self, obj: Object, joint_id: int) -> List[fl def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: """ - Returns the applied torque by a joint motor. + Get the applied torque by a joint motor. :param obj: The object in which the joint is located. :param joint_id: The id of the joint for which the applied motor torque should be returned. @@ -1443,19 +1450,19 @@ def get_applied_joint_motor_torque(self, obj: Object, joint_id: int) -> float: def pause_world_sync(self) -> None: """ - Pauses the world synchronization. + Pause the world synchronization. """ self.world_sync.sync_lock.acquire() def resume_world_sync(self) -> None: """ - Resumes the world synchronization. + Resume the world synchronization. """ self.world_sync.sync_lock.release() def add_vis_axis(self, pose: Pose) -> int: """ - Adds a visual axis to the world. + Add a visual axis to the world. :param pose: The pose of the visual axis. :return: The id of the added visual axis. @@ -1470,7 +1477,7 @@ def _add_vis_axis(self, pose: Pose) -> None: def remove_vis_axis(self) -> None: """ - Removes the visual axis from the world. + Remove the visual axis from the world. """ self._simulator_object_remover(self._remove_vis_axis) @@ -1606,7 +1613,7 @@ def run(self): def get_world_object(self, prospection_object: Object) -> Object: """ - Returns the corresponding object from the main World for a given object in the prospection world. + Get the corresponding object from the main World for a given object in the prospection world. :param prospection_object: The object for which the corresponding object in the main World should be found. :return: The object in the main World. @@ -1620,7 +1627,7 @@ def get_world_object(self, prospection_object: Object) -> Object: def get_prospection_object(self, obj: Object) -> Object: """ - Returns the corresponding object from the prospection world for a given object in the main world. + Get the corresponding object from the prospection world for a given object in the main world. :param obj: The object for which the corresponding object in the prospection World should be found. :return: The corresponding object in the prospection world. diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py index e9b2a60ac..1e7c61e06 100644 --- a/src/pycram/datastructures/world_entity.py +++ b/src/pycram/datastructures/world_entity.py @@ -20,7 +20,7 @@ def __init__(self): @property def saved_states(self) -> Dict[int, State]: """ - Returns the saved states of this entity. + :return: the saved states of this entity. """ return self._saved_states @@ -37,8 +37,6 @@ def save_state(self, state_id: int) -> int: @abstractmethod def current_state(self) -> State: """ - Returns the current state of this entity. - :return: The current state of this entity. """ pass diff --git a/src/pycram/description.py b/src/pycram/description.py index ae4e73c3f..bd3002ed7 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -8,7 +8,7 @@ import rospy import trimesh from geometry_msgs.msg import Point, Quaternion -from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING +from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape from .datastructures.enums import JointType @@ -30,7 +30,7 @@ class EntityDescription(ABC): @abstractmethod def origin(self) -> Pose: """ - Returns the origin of this entity. + :return: the origin of this entity. """ pass @@ -38,7 +38,7 @@ def origin(self) -> Pose: @abstractmethod def name(self) -> str: """ - Returns the name of this entity. + :return: the name of this entity. """ pass @@ -66,6 +66,10 @@ class JointDescription(EntityDescription): """ def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: bool = False): + """ + :param parsed_joint_description: The parsed description of the joint (e.g. from urdf or mjcf file). + :param is_virtual: True if the joint is virtual (i.e. not a physically existing joint), False otherwise. + """ self.parsed_description = parsed_joint_description self.is_virtual: Optional[bool] = is_virtual @@ -89,8 +93,6 @@ def axis(self) -> Point: @abstractmethod def has_limits(self) -> bool: """ - Check if this joint has limits. - :return: True if the joint has limits, False otherwise. """ pass @@ -266,7 +268,7 @@ def current_state(self, link_state: LinkState) -> None: if self.current_state != link_state: self.constraint_ids = link_state.constraint_ids - def add_fixed_constraint_with_link(self, child_link: 'Link', + def add_fixed_constraint_with_link(self, child_link: Self, child_to_parent_transform: Optional[Transform] = None) -> int: """ Add a fixed constraint between this link and the given link, to create attachments for example. @@ -296,8 +298,6 @@ def remove_constraint_with_link(self, child_link: 'Link') -> None: @property def is_only_link(self) -> bool: """ - Whether this link is the only link of the object. - :return: True if this link is the only link, False otherwise. """ return self.object.has_one_link @@ -305,8 +305,6 @@ def is_only_link(self) -> bool: @property def is_root(self) -> bool: """ - Whether this link is the root link of the object. - :return: True if this link is the root link, False otherwise. """ return self.object.get_root_link_id() == self.id @@ -321,8 +319,6 @@ def update_transform(self, transform_time: Optional[rospy.Time] = None) -> None: def get_transform_to_link(self, link: 'Link') -> Transform: """ - Return the transformation from this link to the given link. - :param link: The link to which the transformation should be returned. :return: A Transform object with the transformation from this link to the given link. """ @@ -330,8 +326,6 @@ def get_transform_to_link(self, link: 'Link') -> Transform: def get_transform_from_link(self, link: 'Link') -> Transform: """ - Return the transformation from the given link to this link. - :param link: The link from which the transformation should be returned. :return: A Transform object with the transformation from the given link to this link. """ @@ -339,8 +333,6 @@ def get_transform_from_link(self, link: 'Link') -> Transform: def get_pose_wrt_link(self, link: 'Link') -> Pose: """ - Return the pose of this link with respect to the given link. - :param link: The link with respect to which the pose should be returned. :return: A Pose object with the pose of this link with respect to the given link. """ @@ -348,8 +340,6 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose: def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: """ - Return the axis aligned bounding box of this link. - :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. """ return self.world.get_link_axis_aligned_bounding_box(self) @@ -357,8 +347,6 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: @property def position(self) -> Point: """ - The position of the link relative to the world frame. - :return: A Point object containing the position of the link relative to the world frame. """ return self.pose.position @@ -366,8 +354,6 @@ def position(self) -> Point: @property def position_as_list(self) -> List[float]: """ - The position of the link relative to the world frame as a list. - :return: A list containing the position of the link relative to the world frame. """ return self.pose.position_as_list() @@ -375,8 +361,6 @@ def position_as_list(self) -> List[float]: @property def orientation(self) -> Quaternion: """ - The orientation of the link relative to the world frame. - :return: A Quaternion object containing the orientation of the link relative to the world frame. """ return self.pose.orientation @@ -384,8 +368,6 @@ def orientation(self) -> Quaternion: @property def orientation_as_list(self) -> List[float]: """ - The orientation of the link relative to the world frame as a list. - :return: A list containing the orientation of the link relative to the world frame. """ return self.pose.orientation_as_list() @@ -399,8 +381,6 @@ def update_pose(self) -> None: @property def pose(self) -> Pose: """ - The pose of the link relative to the world frame. - :return: A Pose object containing the pose of the link relative to the world frame. """ if self.world.conf.update_poses_from_sim_on_get: @@ -410,27 +390,24 @@ def pose(self) -> Pose: @property def pose_as_list(self) -> List[List[float]]: """ - The pose of the link relative to the world frame as a list. - :return: A list containing the position and orientation of the link relative to the world frame. """ return self.pose.to_list() def get_origin_transform(self) -> Transform: """ - Returns the transformation between the link frame and the origin frame of this link. + :return: the transformation between the link frame and the origin frame of this link. """ return self.origin.to_transform(self.tf_frame) @property def color(self) -> Color: """ - The rgba_color of this link. - :return: A Color object containing the rgba_color of this link. """ return self.world.get_link_color(self) + @deprecated("Use color property setter instead") def set_color(self, color: Color) -> None: """ Set the color of this link, could be rgb or rgba. @@ -484,7 +461,7 @@ def __init__(self, obj: Object): @property def tf_frame(self) -> str: """ - Return the tf frame of the root link, which is the same as the tf frame of the object. + :return: the tf frame of the root link, which is the same as the tf frame of the object. """ return self.object.tf_frame @@ -519,9 +496,7 @@ def tf_frame(self) -> str: @property def pose(self) -> Pose: """ - Return the pose of this joint. The pose is the pose of the child link of this joint. - - :return: The pose of this joint. + :return: The pose of this joint. The pose is the pose of the child link of this joint. """ return self.child_link.pose @@ -534,8 +509,6 @@ def _update_position(self) -> None: @property def parent_link(self) -> Link: """ - Return the parent link of this joint. - :return: The parent link as a AbstractLink object. """ return self.object.get_link(self.parent) @@ -543,8 +516,6 @@ def parent_link(self) -> Link: @property def child_link(self) -> Link: """ - Return the child link of this joint. - :return: The child link as a AbstractLink object. """ return self.object.get_link(self.child) @@ -561,8 +532,6 @@ def reset_position(self, position: float) -> None: def get_object_id(self) -> int: """ - Return the id of the object to which this joint belongs. - :return: The integer id of the object to which this joint belongs. """ return self.object.id @@ -694,8 +663,6 @@ def joint_map(self) -> Dict[str, JointDescription]: def is_joint_virtual(self, name: str) -> bool: """ - Return whether the joint with the given name is virtual. - :param name: The name of the joint. :return: True if the joint is virtual, False otherwise. """ @@ -748,7 +715,7 @@ def load_description_from_string(self, description_string: str) -> Any: @property def parsed_description(self) -> Any: """ - Return the object parsed from the description file. + :return: The object parsed from the description file. """ return self._parsed_description @@ -830,8 +797,6 @@ def write_description_to_file(description_string: str, save_path: str) -> None: def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: str) -> str: """ - Return the file name of the description file. - :param path_object: The path object of the description file or the mesh file. :param extension: The file extension of the description file or the mesh file. :param object_name: The name of the object. diff --git a/src/pycram/designator.py b/src/pycram/designator.py index d5a8878b2..42ab7a57d 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -365,9 +365,8 @@ def ground(self) -> Any: def get_slots(self) -> List[str]: """ - Returns a list of all slots of this description. Can be used for inspecting different descriptions and debugging. - - :return: A list of all slots. + :return: a list of all slots of this description. Can be used for inspecting different descriptions and + debugging. """ return list(self.__dict__.keys()) @@ -376,7 +375,7 @@ def copy(self) -> DesignatorDescription: def get_default_ontology_concept(self) -> owlready2.Thing | None: """ - Returns the first element of ontology_concept_holders if there is, else None + :return: The first element of ontology_concept_holders if there is, else None """ return self.ontology_concept_holders[0].ontology_concept if self.ontology_concept_holders else None @@ -597,8 +596,6 @@ def insert(self, session: Session) -> ORMObjectDesignator: def frozen_copy(self) -> 'ObjectDesignatorDescription.Object': """ - Returns a copy of this designator containing only the fields. - :return: A copy containing only the fields of this class. The WorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when this method was called. """ result = ObjectDesignatorDescription.Object(self.name, self.obj_type, None) @@ -633,7 +630,7 @@ def __repr__(self): def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: """ - Returns the adjusted target pose based on special knowledge for "grasp front". + Get the adjusted target pose based on special knowledge for "grasp front". :param grasp: From which side the object should be grasped :param pose: Pose at which the object should be grasped, before adjustment diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 19c9805eb..269be7e9b 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/src/pycram/external_interfaces/giskard.py @@ -590,9 +590,7 @@ def allow_gripper_collision(gripper: str) -> None: @init_giskard_interface def get_gripper_group_names() -> List[str]: """ - Returns a list of groups that are registered in giskard which have 'gripper' in their name. - - :return: The list of gripper groups + :return: The list of groups that are registered in giskard which have 'gripper' in their name. """ groups = giskard_wrapper.get_group_names() return list(filter(lambda elem: "gripper" in elem, groups)) @@ -601,7 +599,7 @@ def get_gripper_group_names() -> List[str]: @init_giskard_interface def add_gripper_groups() -> None: """ - Adds the gripper links as a group for collision avoidance. + Add the gripper links as a group for collision avoidance. :return: Response of the RegisterGroup Service """ @@ -645,7 +643,7 @@ def avoid_collisions(object1: Object, object2: Object) -> None: @init_giskard_interface def make_world_body(object: Object) -> 'WorldBody': """ - Creates a WorldBody message for a World Object. The WorldBody will contain the URDF of the World Object + Create a WorldBody message for a World Object. The WorldBody will contain the URDF of the World Object :param object: The World Object :return: A WorldBody message for the World Object diff --git a/src/pycram/helper.py b/src/pycram/helper.py index 4f67e2c4a..d3c39176e 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -29,6 +29,7 @@ def __call__(cls, *args, **kwargs): def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: """ Parse the actuator elements from an MJCF file. + :param file_path: The path to the MJCF file. """ tree = ET.parse(file_path) @@ -49,6 +50,7 @@ def parse_mjcf_actuators(file_path: str) -> Dict[str, str]: def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Optional[str] = None) -> Optional[str]: """ Get the path to the MJCF file of a robot. + :param robot_relative_dir: The relative directory of the robot in the Multiverse resources/robots directory. :param robot_name: The name of the robot. :param xml_name: The name of the XML file of the robot. @@ -75,7 +77,7 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti def find_multiverse_resources_path() -> Optional[str]: """ - Find the path to the Multiverse resources directory. + :return: The path to the Multiverse resources directory. """ # Get the path to the Multiverse installation multiverse_path = find_multiverse_path() @@ -94,7 +96,7 @@ def find_multiverse_resources_path() -> Optional[str]: def find_multiverse_path() -> Optional[str]: """ - Find the path to the Multiverse installation. + :return: the path to the Multiverse installation. """ # Get the value of PYTHONPATH environment variable pythonpath = os.getenv('PYTHONPATH') @@ -110,6 +112,4 @@ def find_multiverse_path() -> Optional[str]: multiverse_path = path.split('multiverse')[0] return multiverse_path + 'multiverse' - return None - diff --git a/src/pycram/local_transformer.py b/src/pycram/local_transformer.py index 9cd58ea5e..d4e83018b 100644 --- a/src/pycram/local_transformer.py +++ b/src/pycram/local_transformer.py @@ -102,7 +102,7 @@ def transform_pose(self, pose: Pose, target_frame: str) -> Optional[Pose]: def get_object_name_for_frame(self, frame: str) -> Optional[str]: """ - Returns the name of the object that is associated with the given frame. + Get the name of the object that is associated with the given frame. :param frame: The frame for which the object name should be returned :return: The name of the object associated with the frame @@ -115,7 +115,7 @@ def get_object_name_for_frame(self, frame: str) -> Optional[str]: def get_object_name_for_link_frame(self, link_frame: str) -> Optional[str]: """ - Returns the name of the object that is associated with the given link frame. + Get the name of the object that is associated with the given link frame. :param link_frame: The frame of the link for which the object name should be returned :return: The name of the object associated with the link frame @@ -155,9 +155,7 @@ def update_transforms(self, transforms: Iterable[Transform], time: rospy.Time = def get_all_frames(self) -> List[str]: """ - Returns all know coordinate frames as a list with human-readable entries. - - :return: A list of all know coordinate frames. + :return: A list of all known coordinate frames as a list with human-readable entries. """ frames = self.allFramesAsString().split("\n") frames.remove("") diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index 232c73d91..d4200db1f 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -23,7 +23,6 @@ from multiverse_parser import MjcfExporter from pxr import Usd, UsdGeom except ImportError: - rospy.logwarn("Multiverse not found.") # do not import this module if multiverse is not found raise ImportError("Multiverse not found.") @@ -39,14 +38,15 @@ def __init__(self, mjcf_description: mjcf.Element): @property def geometry(self) -> Union[VisualShape, None]: """ - Returns the geometry type of the collision element of this link. + :return: The geometry type of the collision element of this link. """ return self._get_visual_shape(self.parsed_description.find_all('geom')[0]) @staticmethod def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: """ - Returns the VisualShape of the given URDF geometry. + :param mjcf_geometry: The MJCFGeometry to get the visual shape for. + :return: The VisualShape of the given MJCFGeometry object. """ if mjcf_geometry.type == MJCFGeomType.BOX.value: return BoxVisualShape(Color(), [0, 0, 0], mjcf_geometry.size) diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 80fa88cfa..79a441ca0 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -32,7 +32,7 @@ def __init__(self, urdf_description: urdf.Link): @property def geometry(self) -> Union[VisualShape, None]: """ - Returns the geometry type of the URDF collision element of this link. + :return: The geometry type of the URDF collision element of this link. """ if self.collision is None: return None @@ -42,7 +42,8 @@ def geometry(self) -> Union[VisualShape, None]: @staticmethod def _get_visual_shape(urdf_geometry) -> Union[VisualShape, None]: """ - Returns the VisualShape of the given URDF geometry. + :param urdf_geometry: The URDFGeometry for which the visual shape is returned. + :return: the VisualShape of the given URDF geometry. """ if isinstance(urdf_geometry, URDF_Box): return BoxVisualShape(Color(), [0, 0, 0], urdf_geometry.size) diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index eca953db6..2e89e303f 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -277,8 +277,6 @@ def __init__(self, robot_name): @staticmethod def get_manager() -> Union[ProcessModuleManager, None]: """ - Returns the Process Module manager for the currently loaded robot or None if there is no Manager. - :return: ProcessModuleManager instance of the current robot """ manager = None @@ -307,7 +305,7 @@ def get_manager() -> Union[ProcessModuleManager, None]: def navigate(self) -> Type[ProcessModule]: """ - Returns the Process Module for navigating the robot with respect to + Get the Process Module for navigating the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for navigating @@ -317,7 +315,7 @@ def navigate(self) -> Type[ProcessModule]: def pick_up(self) -> Type[ProcessModule]: """ - Returns the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Get the Process Module for picking up with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for picking up an object """ @@ -326,7 +324,7 @@ def pick_up(self) -> Type[ProcessModule]: def place(self) -> Type[ProcessModule]: """ - Returns the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type` + Get the Process Module for placing with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for placing an Object """ @@ -335,7 +333,7 @@ def place(self) -> Type[ProcessModule]: def looking(self) -> Type[ProcessModule]: """ - Returns the Process Module for looking at a point with respect to + Get the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for looking at a specific point @@ -345,7 +343,7 @@ def looking(self) -> Type[ProcessModule]: def detecting(self) -> Type[ProcessModule]: """ - Returns the Process Module for detecting an object with respect to + Get the Process Module for detecting an object with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for detecting an object @@ -355,7 +353,7 @@ def detecting(self) -> Type[ProcessModule]: def move_tcp(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the Tool Center Point with respect to + Get the Process Module for moving the Tool Center Point with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the TCP @@ -365,7 +363,7 @@ def move_tcp(self) -> Type[ProcessModule]: def move_arm_joints(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the joints of the robot arm + Get the Process Module for moving the joints of the robot arm with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the arm joints @@ -375,7 +373,7 @@ def move_arm_joints(self) -> Type[ProcessModule]: def world_state_detecting(self) -> Type[ProcessModule]: """ - Returns the Process Module for detecting an object using the world state with respect to the + Get the Process Module for detecting an object using the world state with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for world state detecting @@ -385,7 +383,7 @@ def world_state_detecting(self) -> Type[ProcessModule]: def move_joints(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving any joint of the robot with respect to the + Get the Process Module for moving any joint of the robot with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving joints @@ -395,7 +393,7 @@ def move_joints(self) -> Type[ProcessModule]: def move_gripper(self) -> Type[ProcessModule]: """ - Returns the Process Module for moving the gripper with respect to + Get the Process Module for moving the gripper with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for moving the gripper @@ -405,7 +403,7 @@ def move_gripper(self) -> Type[ProcessModule]: def open(self) -> Type[ProcessModule]: """ - Returns the Process Module for opening drawers with respect to + Get the Process Module for opening drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for opening drawers @@ -415,7 +413,7 @@ def open(self) -> Type[ProcessModule]: def close(self) -> Type[ProcessModule]: """ - Returns the Process Module for closing drawers with respect to + Get the Process Module for closing drawers with respect to the :py:attr:`~ProcessModuleManager.execution_type` :return: The Process Module for closing drawers diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index dad64f8e7..c6e41f9d3 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -159,7 +159,7 @@ def has_actuators(self): def get_actuator_for_joint(self, joint: str) -> Optional[str]: """ - Returns the actuator name for a given joint. + Get the actuator name for a given joint. :param joint: Name of the joint :return: Name of the actuator @@ -238,7 +238,7 @@ def add_grasp_orientations(self, orientations: Dict[Grasp, List[float]]): def get_manipulator_chains(self) -> List[KinematicChainDescription]: """ - Returns a list of all manipulator chains of the robot which posses an end effector. + Get a list of all manipulator chains of the robot which posses an end effector. :return: A list of KinematicChainDescription objects """ @@ -266,7 +266,7 @@ def get_camera_frame(self) -> str: def get_default_camera(self) -> CameraDescription: """ - Returns the first camera in the list of cameras. + Get the first camera in the list of cameras. :return: A CameraDescription object """ @@ -274,7 +274,7 @@ def get_default_camera(self) -> CameraDescription: def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: str): """ - Returns the static joint states of a kinematic chain for a specific configuration. When trying to access one of + Get the static joint states of a kinematic chain for a specific configuration. When trying to access one of the robot arms the function `:func: get_arm_chain` should be used. :param kinematic_chain_name: @@ -292,7 +292,7 @@ def get_static_joint_chain(self, kinematic_chain_name: str, configuration_name: def get_parent(self, name: str) -> str: """ - Returns the parent of a link or joint in the URDF. Always returns the imeadiate parent, for a link this is a joint + Get the parent of a link or joint in the URDF. Always returns the imeadiate parent, for a link this is a joint and vice versa. :param name: Name of the link or joint in the URDF @@ -312,7 +312,7 @@ def get_parent(self, name: str) -> str: def get_child(self, name: str, return_multiple_children: bool = False) -> Union[str, List[str]]: """ - Returns the child of a link or joint in the URDF. Always returns the immediate child, for a link this is a joint + Get the child of a link or joint in the URDF. Always returns the immediate child, for a link this is a joint and vice versa. Since a link can have multiple children, the return_multiple_children parameter can be set to True to get a list of all children. @@ -341,7 +341,7 @@ def get_child(self, name: str, return_multiple_children: bool = False) -> Union[ def get_arm_tool_frame(self, arm: Arms) -> str: """ - Returns the name of the tool frame of a specific arm. + Get the name of the tool frame of a specific arm. :param arm: Arm for which the tool frame should be returned :return: The name of the link of the tool frame in the URDF. @@ -351,7 +351,7 @@ def get_arm_tool_frame(self, arm: Arms) -> str: def get_arm_chain(self, arm: Arms) -> Union[KinematicChainDescription, List[KinematicChainDescription]]: """ - Returns the kinematic chain of a specific arm. If the arm is set to BOTH, all kinematic chains are returned. + Get the kinematic chain of a specific arm. If the arm is set to BOTH, all kinematic chains are returned. :param arm: Arm for which the chain should be returned :return: KinematicChainDescription object of the arm @@ -456,7 +456,7 @@ def _init_joints(self): def get_joints(self) -> List[str]: """ - Returns a list of all joints of the chain. + Get a list of all joints of the chain. :return: List of joint names """ @@ -464,9 +464,7 @@ def get_joints(self) -> List[str]: def get_links(self) -> List[str]: """ - Returns a list of all links of the chain. - - :return: List of link names + :return: A list of all links of the chain. """ return self.link_names @@ -502,7 +500,7 @@ def add_static_joint_states(self, name: str, states: dict): def get_static_joint_states(self, name: str) -> Dict[str, float]: """ - Returns the dictionary of static joint states for a given name of the static joint states. + Get the dictionary of static joint states for a given name of the static joint states. :param name: Name of the static joint states :return: Dictionary of joint names and their values @@ -514,7 +512,7 @@ def get_static_joint_states(self, name: str) -> Dict[str, float]: def get_tool_frame(self) -> str: """ - Returns the name of the tool frame of the end effector of this chain, if it has an end effector. + Get the name of the tool frame of the end effector of this chain, if it has an end effector. :return: The name of the link of the tool frame in the URDF. """ @@ -525,7 +523,7 @@ def get_tool_frame(self) -> str: def get_static_gripper_state(self, state: GripperState) -> Dict[str, float]: """ - Returns the static joint states for the gripper of the chain. + Get the static joint states for the gripper of the chain. :param state: Name of the static joint states :return: Dictionary of joint names and their values diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py index 9f504a840..8de36bda1 100644 --- a/src/pycram/world_concepts/constraints.py +++ b/src/pycram/world_concepts/constraints.py @@ -2,7 +2,7 @@ import numpy as np from geometry_msgs.msg import Point -from typing_extensions import Union, List, Optional, TYPE_CHECKING +from typing_extensions import Union, List, Optional, TYPE_CHECKING, Self from ..datastructures.enums import JointType from ..datastructures.pose import Transform, Pose @@ -32,15 +32,13 @@ def __init__(self, def get_child_object_pose(self) -> Pose: """ - Returns the pose of the child object. - :return: The pose of the child object. """ return self.child_link.object.pose def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: """ - Returns the pose of the child object given the parent pose. + Get the pose of the child object given the parent pose. :param pose: The parent object pose. :return: The pose of the child object. @@ -51,19 +49,19 @@ def get_child_object_pose_given_parent(self, pose: Pose) -> Pose: def set_child_link_pose(self): """ - Sets the target pose of the child object to the current pose of the child object in the parent object frame. + Set the target pose of the child object to the current pose of the child object in the parent object frame. """ self.child_link.set_pose(self.get_child_link_target_pose()) def get_child_link_target_pose(self) -> Pose: """ - Returns the target pose of the child object. (The pose of the child object in the parent object frame) + :return: The target pose of the child object. (The pose of the child object in the parent object frame) """ return self.parent_to_child_transform.to_pose() def get_child_link_target_pose_given_parent(self, parent_pose: Pose) -> Pose: """ - Returns the target pose of the child object link given the parent link pose. + Get the target pose of the child object link given the parent link pose. :param parent_pose: The parent link pose. :return: The target pose of the child object link. @@ -84,8 +82,6 @@ def parent_to_child_transform(self, transform: Transform) -> None: @property def parent_object_id(self) -> int: """ - Returns the id of the parent object of the constraint. - :return: The id of the parent object of the constraint """ return self.parent_link.object_id @@ -93,8 +89,6 @@ def parent_object_id(self) -> int: @property def child_object_id(self) -> int: """ - Returns the id of the child object of the constraint. - :return: The id of the child object of the constraint """ return self.child_link.object_id @@ -102,8 +96,6 @@ def child_object_id(self) -> int: @property def parent_link_id(self) -> int: """ - Returns the id of the parent link of the constraint. - :return: The id of the parent link of the constraint """ return self.parent_link.id @@ -111,8 +103,6 @@ def parent_link_id(self) -> int: @property def child_link_id(self) -> int: """ - Returns the id of the child link of the constraint. - :return: The id of the child link of the constraint """ return self.child_link.id @@ -120,8 +110,6 @@ def child_link_id(self) -> int: @property def position_wrt_parent_as_list(self) -> List[float]: """ - Returns the constraint frame pose with respect to the parent origin as a list. - :return: The constraint frame pose with respect to the parent origin as a list """ return self.pose_wrt_parent.position_as_list() @@ -129,8 +117,6 @@ def position_wrt_parent_as_list(self) -> List[float]: @property def orientation_wrt_parent_as_list(self) -> List[float]: """ - Returns the constraint frame orientation with respect to the parent origin as a list. - :return: The constraint frame orientation with respect to the parent origin as a list """ return self.pose_wrt_parent.orientation_as_list() @@ -138,8 +124,6 @@ def orientation_wrt_parent_as_list(self) -> List[float]: @property def pose_wrt_parent(self) -> Pose: """ - Returns the joint frame pose with respect to the parent origin. - :return: The joint frame pose with respect to the parent origin """ return self.parent_to_constraint.to_pose() @@ -147,8 +131,6 @@ def pose_wrt_parent(self) -> Pose: @property def position_wrt_child_as_list(self) -> List[float]: """ - Returns the constraint frame pose with respect to the child origin as a list. - :return: The constraint frame pose with respect to the child origin as a list """ return self.pose_wrt_child.position_as_list() @@ -156,8 +138,6 @@ def position_wrt_child_as_list(self) -> List[float]: @property def orientation_wrt_child_as_list(self) -> List[float]: """ - Returns the constraint frame orientation with respect to the child origin as a list. - :return: The constraint frame orientation with respect to the child origin as a list """ return self.pose_wrt_child.orientation_as_list() @@ -165,8 +145,6 @@ def orientation_wrt_child_as_list(self) -> List[float]: @property def pose_wrt_child(self) -> Pose: """ - Returns the joint frame pose with respect to the child origin. - :return: The joint frame pose with respect to the child origin """ return self.child_to_constraint.to_pose() @@ -191,8 +169,6 @@ def __init__(self, @property def axis_as_list(self) -> List[float]: """ - Returns the axis of this constraint as a list. - :return: The axis of this constraint as a list of xyz """ return [self.axis.x, self.axis.y, self.axis.z] @@ -242,40 +218,40 @@ def child_object(self): def update_transform_and_constraint(self) -> None: """ - Updates the transform and constraint of this attachment. + Update the transform and constraint of this attachment. """ self.update_transform() self.update_constraint() def update_transform(self) -> None: """ - Updates the transform of this attachment by calculating the transform from the parent link to the child link. + Update the transform of this attachment by calculating the transform from the parent link to the child link. """ self.parent_to_child_transform = self.calculate_transform() def update_constraint(self) -> None: """ - Updates the constraint of this attachment by removing the old constraint if one exists and adding a new one. + Update the constraint of this attachment by removing the old constraint if one exists and adding a new one. """ self.remove_constraint_if_exists() self.add_fixed_constraint() def add_fixed_constraint(self) -> None: """ - Adds a fixed constraint between the parent link and the child link. + Add a fixed constraint between the parent link and the child link. """ self.id = self.parent_link.add_fixed_constraint_with_link(self.child_link, self.parent_to_child_transform.invert()) def calculate_transform(self) -> Transform: """ - Calculates the transform from the parent link to the child link. + Calculate the transform from the parent link to the child link. """ return self.parent_link.get_transform_to_link(self.child_link) def remove_constraint_if_exists(self) -> None: """ - Removes the constraint between the parent and the child links if one exists. + Remove the constraint between the parent and the child links if one exists. """ if self.child_link in self.parent_link.constraint_ids: self.parent_link.remove_constraint_with_link(self.child_link) @@ -299,7 +275,7 @@ def loose(self) -> bool: @loose.setter def loose(self, loose: bool) -> None: """ - Sets the loose property of this attachment. + Set the loose property of this attachment. :param loose: If true, then the child object will not move when parent moves. """ @@ -307,7 +283,7 @@ def loose(self, loose: bool) -> None: def __del__(self) -> None: """ - Removes the constraint between the parent and the child links if one exists when the attachment is deleted. + Remove the constraint between the parent and the child links if one exists when the attachment is deleted. """ self.remove_constraint_if_exists() diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py index 8cea3d5f8..8fd0501b9 100644 --- a/src/pycram/world_reasoning.py +++ b/src/pycram/world_reasoning.py @@ -264,7 +264,7 @@ def link_pose_for_joint_config( joint_config: Dict[str, float], link_name: str) -> Pose: """ - Returns the pose a link would be in if the given joint configuration would be applied to the object. + Get the pose a link would be in if the given joint configuration would be applied to the object. This is done by using the respective object in the prospection world and applying the joint configuration to this one. After applying the joint configuration the link position is taken from there. diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 4cda8cedc..c92e5289e 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -93,7 +93,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, if not self.is_prospection_world: self._spawn_floor() - if self.conf.use_bullet_mode: + if self.conf.use_static_mode: self.api_requester.pause_simulation() def _init_clients(self, is_prospection: bool = False): @@ -603,9 +603,9 @@ def ray_test_batch(self, from_positions: List[List[float]], def step(self): """ - Perform a simulation step in the simulator, this is useful when use_bullet_mode is True. + Perform a simulation step in the simulator, this is useful when use_static_mode is True. """ - if self.conf.use_bullet_mode: + if self.conf.use_static_mode: self.api_requester.unpause_simulation() sleep(self.simulation_time_step) self.api_requester.pause_simulation() diff --git a/src/pycram/worlds/multiverse_communication/client_manager.py b/src/pycram/worlds/multiverse_communication/client_manager.py index 6d52f51fb..a1b768172 100644 --- a/src/pycram/worlds/multiverse_communication/client_manager.py +++ b/src/pycram/worlds/multiverse_communication/client_manager.py @@ -25,7 +25,7 @@ def __init__(self, simulation_wait_time_factor: Optional[float] = 1.0): """ self.simulation_wait_time_factor = simulation_wait_time_factor - def create_reader(self, is_prospection_world: Optional[bool] = False) -> 'MultiverseReader': + def create_reader(self, is_prospection_world: Optional[bool] = False) -> MultiverseReader: """ Create a Multiverse reader client. From 78f518007cceb268bdd5d4232906fbd66dd85f9e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 25 Sep 2024 11:42:43 +0200 Subject: [PATCH 186/189] [MultiverseDev] moved simulation_frequency to config file instead of world init argument and class variable. --- config/world_conf.py | 5 +++++ src/pycram/datastructures/world.py | 17 ++++------------- src/pycram/worlds/bullet_world.py | 4 ++-- src/pycram/worlds/multiverse.py | 4 +--- 4 files changed, 12 insertions(+), 18 deletions(-) diff --git a/config/world_conf.py b/config/world_conf.py index 7d11942c9..76db4b330 100644 --- a/config/world_conf.py +++ b/config/world_conf.py @@ -40,6 +40,11 @@ class WorldConfig: The prefix for the prospection world name. """ + simulation_frequency: int = 240 + """ + The simulation frequency (Hz), used for calculating the equivalent real time in the simulation. + """ + update_poses_from_sim_on_get: bool = True """ Whether to update the poses from the simulator when getting the object poses. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a152a4938..47e2d1bd9 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -52,11 +52,6 @@ class World(StateEntity, ABC): The configurations of the world, the default configurations are defined in world_conf.py in the config folder. """ - simulation_frequency: float - """ - Global reference for the simulation frequency (Hz), used in calculating the equivalent real time in the simulation. - """ - current_world: Optional[World] = None """ Global reference to the currently used World, usually this is the @@ -76,8 +71,7 @@ class World(StateEntity, ABC): Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ - def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequency: float, - clear_cache: bool = False): + def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_cache: bool = False): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -86,7 +80,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is "GUI" :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world. - :param simulation_frequency: The frequency of the simulation in Hz. :param clear_cache: Whether to clear the cache directory. """ @@ -96,7 +89,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ self.cache_manager.clear_cache() GoalValidator.raise_error = self.conf.raise_goal_validator_error - World.simulation_frequency = simulation_frequency if World.current_world is None: World.current_world = self @@ -252,8 +244,7 @@ def _init_prospection_world(self): self.prospection_world = None else: self.prospection_world: World = self.__class__(WorldMode.DIRECT, - True, - World.simulation_frequency) + True) def _sync_prospection_world(self): """ @@ -287,7 +278,7 @@ def simulation_time_step(self): """ The time step of the simulation in seconds. """ - return 1 / self.__class__.simulation_frequency + return 1 / self.__class__.conf.simulation_frequency @abstractmethod def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, @@ -561,7 +552,7 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False) -> None: :param real_time: If the simulation should happen in real time or faster. """ self.set_realtime(real_time) - for i in range(0, int(seconds * self.simulation_frequency)): + for i in range(0, int(seconds * self.conf.simulation_frequency)): curr_time = rospy.Time.now() self.step() for objects, callbacks in self.coll_callbacks.items(): diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 36443ebff..e20e7ad3c 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -39,7 +39,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s if rosgraph.is_master_online(): # and "/pycram" not in rosnode.get_node_names(): rospy.init_node('pycram') - def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False, sim_frequency=240): + def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False): """ Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the background. There can only be one rendered simulation. @@ -48,7 +48,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI" :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world. """ - super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_frequency=sim_frequency) + super().__init__(mode=mode, is_prospection_world=is_prospection_world) # This disables file caching from PyBullet, since this would also cache # files that can not be loaded diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index c92e5289e..fd174155b 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -57,7 +57,6 @@ class Multiverse(World): def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, is_prospection: Optional[bool] = False, - simulation_frequency: float = conf.simulation_frequency, simulation_name: str = "pycram_test", clear_cache: bool = False): """ @@ -65,7 +64,6 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, :param mode: The mode of the world (DIRECT or GUI). :param is_prospection: Whether the world is prospection or not. - :param simulation_frequency: The frequency of the simulation. :param simulation_name: The name of the simulation. :param clear_cache: Whether to clear the cache or not. """ @@ -84,7 +82,7 @@ def __init__(self, mode: Optional[WorldMode] = WorldMode.DIRECT, self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor) self._init_clients(is_prospection=is_prospection) - World.__init__(self, mode, is_prospection, simulation_frequency) + World.__init__(self, mode, is_prospection) self._init_constraint_and_object_id_name_map_collections() From 901ed295adf48999f35217d7deda4ae87a7c9863 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 26 Sep 2024 15:55:06 +0200 Subject: [PATCH 187/189] [MultiverseDev] added NamedBoxVisualShape for generic objects. fixed viz_marker_publisher.py color for one link objects fixed viz_marker_publisher.py size of box visual shape objects. --- src/pycram/object_descriptors/generic.py | 21 +++++++++++++++------ src/pycram/ros/viz_marker_publisher.py | 6 ++++-- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py index 9e94b645d..fb2b456ec 100644 --- a/src/pycram/object_descriptors/generic.py +++ b/src/pycram/object_descriptors/generic.py @@ -11,12 +11,21 @@ ObjectDescription as AbstractObjectDescription +class NamedBoxVisualShape(BoxVisualShape): + def __init__(self, name: str, color: Color, visual_frame_position: List[float], half_extents: List[float]): + super().__init__(color, visual_frame_position, half_extents) + self._name: str = name + + @property + def name(self) -> str: + return self._name + + class LinkDescription(AbstractLinkDescription): def __init__(self, name: str, visual_frame_position: List[float], half_extents: List[float], color: Color = Color()): - self.parsed_description: BoxVisualShape = BoxVisualShape(color, visual_frame_position, half_extents) - self._name: str = name + super().__init__(NamedBoxVisualShape(name, color, visual_frame_position, half_extents)) @property def geometry(self) -> Union[VisualShape, None]: @@ -28,7 +37,7 @@ def origin(self) -> Pose: @property def name(self) -> str: - return self._name + return self.parsed_description.name @property def color(self) -> Color: @@ -104,15 +113,15 @@ def load_description(self, path: str) -> Any: ... @classmethod - def generate_from_mesh_file(cls, path: str, name: str) -> str: + def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> str: raise NotImplementedError @classmethod - def generate_from_description_file(cls, path: str, make_mesh_paths_absolute: bool = True) -> str: + def generate_from_description_file(cls, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> str: raise NotImplementedError @classmethod - def generate_from_parameter_server(cls, name: str) -> str: + def generate_from_parameter_server(cls, name: str, save_path: str) -> str: raise NotImplementedError @property diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 7b7eca5f5..39e519b3d 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -3,6 +3,7 @@ import time from typing import List, Optional, Tuple +import numpy as np import rospy from geometry_msgs.msg import Vector3 from std_msgs.msg import ColorRGBA @@ -80,7 +81,7 @@ def _make_marker_array(self) -> MarkerArray: link_pose_with_origin = link_pose * link_origin msg.pose = link_pose_with_origin.to_pose().pose - color = [1, 1, 1, 1] if obj.link_name_to_id[link] == -1 else obj.get_link_color(link).get_rgba() + color = obj.get_link_color(link).get_rgba() msg.color = ColorRGBA(*color) msg.lifetime = rospy.Duration(1) @@ -95,7 +96,8 @@ def _make_marker_array(self) -> MarkerArray: msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.length) elif isinstance(geom, BoxVisualShape): msg.type = Marker.CUBE - msg.scale = Vector3(*geom.size) + size = np.array(geom.size) * 2 + msg.scale = Vector3(size[0], size[1], size[2]) elif isinstance(geom, SphereVisualShape): msg.type = Marker.SPHERE msg.scale = Vector3(geom.radius * 2, geom.radius * 2, geom.radius * 2) From 2ed35e9ee642a9fb0d6db82e801047310b83a1f0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 26 Sep 2024 16:48:02 +0200 Subject: [PATCH 188/189] [MultiverseDev] object path can be None, for e.g. when using generic objects. --- src/pycram/world_concepts/world_object.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 147e6e906..955c6691e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -49,7 +49,7 @@ class Object(WorldEntity): A dictionary that maps the file extension to the corresponding ObjectDescription type. """ - def __init__(self, name: str, obj_type: ObjectType, path: str, + def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, @@ -65,7 +65,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, :param name: The name of the object :param obj_type: The type of the object as an ObjectType enum. :param path: The path to the source file, if only a filename is provided then the resources directories will be - searched. + searched, it could be None in some cases when for example it is a generic object. :param description: The ObjectDescription of the object, this contains the joints and links of the object. :param pose: The pose at which the Object should be spawned :param world: The World in which the object should be spawned, if no world is specified the From 36c1036100998cca0cf4dcad38b5c3f812f24014 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 26 Sep 2024 17:12:07 +0200 Subject: [PATCH 189/189] [MultiverseDev] fixed urdf box visual shape size. Allowed generic objects to take and set its pose when loaded. --- src/pycram/datastructures/world.py | 6 +++++- src/pycram/object_descriptors/urdf.py | 4 +++- src/pycram/world_concepts/world_object.py | 2 +- src/pycram/worlds/bullet_world.py | 10 ++++++++-- 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 47e2d1bd9..d9c16e5e9 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -293,9 +293,13 @@ def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose """ pass - def load_generic_object_and_get_id(self, description: GenericObjectDescription) -> int: + def load_generic_object_and_get_id(self, description: GenericObjectDescription, + pose: Optional[Pose] = None) -> int: """ Create a visual and collision box in the simulation and returns the id of the loaded object. + + :param description: The object description. + :param pose: The pose at which the object should be loaded. """ raise NotImplementedError diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index 79a441ca0..50d11608f 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -2,6 +2,7 @@ import pathlib import xml.etree.ElementTree as ET +import numpy as np import rospkg import rospy from geometry_msgs.msg import Point @@ -46,7 +47,8 @@ def _get_visual_shape(urdf_geometry) -> Union[VisualShape, None]: :return: the VisualShape of the given URDF geometry. """ if isinstance(urdf_geometry, URDF_Box): - return BoxVisualShape(Color(), [0, 0, 0], urdf_geometry.size) + half_extents = np.array(urdf_geometry.size) / 2 + return BoxVisualShape(Color(), [0, 0, 0], half_extents.tolist()) if isinstance(urdf_geometry, URDF_Cylinder): return CylinderVisualShape(Color(), [0, 0, 0], urdf_geometry.radius, urdf_geometry.length) if isinstance(urdf_geometry, URDF_Sphere): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 955c6691e..4975fa983 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -296,7 +296,7 @@ def _spawn_object_and_get_id(self) -> int: :return: The unique id of the object and the path of the file that was loaded. """ if isinstance(self.description, GenericObjectDescription): - return self.world.load_generic_object_and_get_id(self.description) + return self.world.load_generic_object_and_get_id(self.description, pose=self._current_pose) path = self.path if self.world.conf.let_pycram_handle_spawning else self.name diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index e20e7ad3c..c80e925f1 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -70,7 +70,8 @@ def _init_world(self, mode: WorldMode): self._gui_thread.start() time.sleep(0.1) - def load_generic_object_and_get_id(self, description: GenericObjectDescription) -> int: + def load_generic_object_and_get_id(self, description: GenericObjectDescription, + pose: Optional[Pose] = None) -> int: """ Creates a visual and collision box in the simulation. """ @@ -86,6 +87,8 @@ def load_generic_object_and_get_id(self, description: GenericObjectDescription) basePosition=description.origin.position_as_list(), baseOrientation=description.origin.orientation_as_list(), physicsClientId=self.id) + if pose is not None: + self._set_object_pose_by_id(obj_id, pose) # Assuming you have a list to keep track of created objects return obj_id @@ -238,7 +241,10 @@ def reset_multiple_objects_base_poses(self, objects: Dict[Object, Pose]) -> bool @validate_object_pose def reset_object_base_pose(self, obj: Object, pose: Pose) -> bool: - p.resetBasePositionAndOrientation(obj.id, pose.position_as_list(), pose.orientation_as_list(), + return self._set_object_pose_by_id(obj.id, pose) + + def _set_object_pose_by_id(self, obj_id: int, pose: Pose) -> bool: + p.resetBasePositionAndOrientation(obj_id, pose.position_as_list(), pose.orientation_as_list(), physicsClientId=self.id) return True