From c5bd732d40863e128aaf4a30edde9b806749bfa0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 21 Aug 2024 16:50:42 +0200 Subject: [PATCH] [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