Skip to content

Commit

Permalink
[MultiverseController] merged dev and fixed issues.
Browse files Browse the repository at this point in the history
  • Loading branch information
AbdelrhmanBassiouny committed Aug 21, 2024
1 parent 4bff422 commit c5bd732
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 11 deletions.
7 changes: 7 additions & 0 deletions src/pycram/datastructures/world.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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.
Expand Down
37 changes: 32 additions & 5 deletions src/pycram/object_descriptors/generic.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
from typing import Optional
from typing import Optional, Tuple

from typing_extensions import List, Any, Union, Dict

Expand All @@ -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

Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
12 changes: 6 additions & 6 deletions src/pycram/world_concepts/world_object.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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()
Expand Down Expand Up @@ -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

Expand Down

0 comments on commit c5bd732

Please sign in to comment.