Abstract world

demos/pycram_bullet_world_demo/demo.py

@@ -1,22 +1,28 @@
+from pycram.bullet_world import BulletWorld
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.designators.object_designator import *
+from pycram.enums import ObjectType
 from pycram.pose import Pose
-from pycram.bullet_world import BulletWorld
+from pycram.process_module import simulated_robot, with_simulated_robot
+from pycram.urdf_interface import ObjectDescription
 from pycram.world import Object
 from pycram.world_dataclasses import Color
-from pycram.process_module import simulated_robot, with_simulated_robot
-from pycram.enums import ObjectType
+
+extension = ObjectDescription.get_file_extension()
 
 world = BulletWorld()
-robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0]))
-apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf")
-
-milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1))
-cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]),
-                color=[0, 1, 0, 1])
-spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=Color(0, 0, 1, 1))
-bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=Color(1, 1, 0, 1))
+robot = Object("pr2", ObjectType.ROBOT, f"pr2{extension}", ObjectDescription, pose=Pose([1, 2, 0]))
+apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}", ObjectDescription)
+
+milk = Object("milk", ObjectType.MILK, "milk.stl", ObjectDescription, pose=Pose([2.5, 2, 1.02]),
+              color=Color(1, 0, 0, 1))
+cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", ObjectDescription,
+                pose=Pose([2.5, 2.3, 1.05]), color=Color(0, 1, 0, 1))
+spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", ObjectDescription, pose=Pose([2.4, 2.2, 0.85]),
+               color=Color(0, 0, 1, 1))
+bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", ObjectDescription, 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])
@@ -55,7 +61,8 @@ def move_and_detect(obj_type):
 
     # 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()
+    drawer_open_loc = AccessingLocation(handle_desig=handle_desig.resolve(),
+                                        robot_desig=robot_desig.resolve()).resolve()
 
     NavigateAction([drawer_open_loc.pose]).resolve().perform()
 
@@ -91,4 +98,3 @@ def move_and_detect(obj_type):
     PlaceAction(spoon_desig, [spoon_target_pose], [pickup_arm]).resolve().perform()
 
     ParkArmsAction([Arms.BOTH]).resolve().perform()
-

src/pycram/cache_manager.py

@@ -0,0 +1,123 @@
+import os
+import pathlib
+import re
+
+from typing_extensions import List
+
+
+class CacheManager:
+
+    mesh_extensions: List[str] = [".obj", ".stl"]
+    """
+    The file extensions of mesh files.
+    """
+
+    def __init__(self, cache_dir: str, data_directory: List[str]):
+        self.cache_dir = cache_dir
+        # The directory where the cached files are stored.
+
+        self.data_directory = data_directory
+        # The directory where all resource files are stored.
+
+    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.
+        """
+        path_object = pathlib.Path(path)
+        extension = path_object.suffix
+
+        path = self.look_for_file_in_data_dir(path, path_object)
+
+        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)
+
+        # if file is not yet cached preprocess the description file and save it in the cache directory.
+        if not self.is_cached(path, object_description) or ignore_cached_files:
+            self.generate_description_and_write_to_cache(path, extension, cache_path, object_description)
+
+        return cache_path
+
+    def generate_description_and_write_to_cache(self, path: 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.
+        :param path: The path of the file to preprocess.
+        :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, extension)
+        self.write_to_cache(description_string, cache_path)
+
+    @staticmethod
+    def write_to_cache(description_string: str, cache_path: str) -> None:
+        """
+        Writes 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: str, 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.
+        :param path: The path of the file to look for.
+        :param path_object: The pathlib object of the file to look for.
+        """
+        if re.match("[a-zA-Z_0-9].[a-zA-Z0-9]", path):
+            for data_dir in self.data_directory:
+                for file in os.listdir(data_dir):
+                    if file == path:
+                        return data_dir + f"/{path}"
+                if path:
+                    break
+
+        if not path:
+            raise FileNotFoundError(
+                f"File {path_object.name} could not be found in the resource directory {self.data_directory}")
+
+        return path
+
+    def create_cache_dir_if_not_exists(self):
+        """
+        Creates 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
+        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.
+
+        :param path: The path of the file to check.
+        :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)
+
+    def check_with_extension(self, path: str) -> bool:
+        """
+        Checks if the file in the given ath exists in the cache directory including file extension.
+        :param path: The path of the file to check.
+        """
+        file_name = pathlib.Path(path).name
+        full_path = pathlib.Path(self.cache_dir + file_name)
+        return full_path.exists()
+
+    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.
+        :param path: The path of the file to check.
+        :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())
+        return full_path.exists()

src/pycram/costmaps.py

@@ -1,7 +1,7 @@
 # used for delayed evaluation of typing until python 3.11 becomes mainstream
 from __future__ import annotations
 
-from typing import Tuple, List, Optional
+from typing_extensions import Tuple, List, Optional
 
 import matplotlib.pyplot as plt
 import numpy as np
@@ -10,11 +10,13 @@
 from matplotlib import colors
 from nav_msgs.msg import OccupancyGrid, MapMetaData
 
-from pycram.world import UseProspectionWorld, Object, Link
+from .world import UseProspectionWorld
+from .world_object import Object
+from .description import Link
 from .local_transformer import LocalTransformer
 from .pose import Pose, Transform
 from .world import World
-from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, BoxShapeData, MultiBody, Color, JointType
+from .world_dataclasses import AxisAlignedBoundingBox, BoxVisualShape, Color
 
 
 class Costmap:
@@ -54,8 +56,7 @@ def __init__(self, resolution: float,
     def visualize(self) -> None:
         """
         Visualizes a costmap in the World, the visualisation works by
-        subdividing the costmap in rectangles which are then visualized as pybullet
-        visual shapes.
+        subdividing the costmap in rectangles which are then visualized as world visual shapes.
         """
         if self.vis_ids:
             return
@@ -75,43 +76,26 @@ def visualize(self) -> None:
                     map[i:i + curr_height, j:j + curr_width] = 0
         cells = []
         # 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
+        # please look here:
+        # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.q1gn7v6o58bf
         for box in boxes:
-            box_shape_data = BoxShapeData([(box[1] * self.resolution) / 2, (box[2] * self.resolution) / 2, 0.001])
-            visual_frame_position = [(box[0][0] + box[1] / 2) * self.resolution,
-                                     (box[0][1] + box[2] / 2) * self.resolution, 0.]
-            visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6), visual_frame_position, box_shape_data)
+            visual_shape = BoxVisualShape(Color(1, 0, 0, 0.6),
+                                          visual_frame_position=[(box[0][0] + box[1] / 2) * self.resolution,
+                                                                 (box[0][1] + box[2] / 2) * self.resolution, 0.],
+                                          half_extents=[(box[1] * self.resolution) / 2,
+                                                        (box[2] * self.resolution) / 2, 0.001])
             visual = self.world.create_visual_shape(visual_shape)
             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 = [JointType.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]
-
             offset = Transform([-self.height / 2 * self.resolution, -self.width / 2 * self.resolution, 0.05],
                                [0, 0, 0, 1])
             origin = Transform(self.origin.position_as_list(), self.origin.orientation_as_list())
             new_transform = origin * offset
             new_pose = new_transform.to_pose().to_list()
 
-            multi_body = MultiBody(base_visual_shape_index=-1, base_position=new_pose[0], base_orientation=new_pose[1],
-                                   link_visual_shape_indices=cell_parts, link_positions=link_poses,
-                                   link_orientations=link_orientations, link_masses=link_masses,
-                                   link_inertial_frame_positions=link_poses,
-                                   link_inertial_frame_orientations=link_orientations,
-                                   link_parent_indices=link_parent, link_joint_types=link_joints,
-                                   link_joint_axis=link_joint_axis,
-                                   link_collision_shape_indices=link_collision)
-
-            map_obj = self.world.create_multi_body(multi_body)
+            map_obj = self.world.create_multi_body_from_visual_shapes(cell_parts, Pose(*new_pose))
             self.vis_ids.append(map_obj)
 
     def _chunks(self, lst: List, n: int) -> List:
@@ -130,7 +114,7 @@ def close_visualization(self) -> None:
         Removes the visualization from the World.
         """
         for v_id in self.vis_ids:
-            self.world.remove_object(v_id)
+            self.world.remove_object(self.world.get_object_by_id(v_id))
         self.vis_ids = []
 
     def _find_consectuive_line(self, start: Tuple[int, int], map: np.ndarray) -> int:
@@ -266,7 +250,7 @@ def __init__(self, distance_to_obstacle: float,
             self.origin = Pose() if not origin else origin
             self.resolution = resolution
             self.distance_obstacle = max(int(distance_to_obstacle / self.resolution), 1)
-            self.map = self._create_from_bullet(size, resolution)
+            self.map = self._create_from_world(size, resolution)
             Costmap.__init__(self, resolution, size, size, self.origin, self.map)
 
     def _calculate_diff_origin(self, height: int, width: int) -> Pose:
@@ -361,7 +345,7 @@ def create_sub_map(self, sub_origin: Pose, size: int) -> Costmap:
         sub_map = np.rot90(np.flip(self._convert_map(sub_map), 0))
         return Costmap(self.resolution, size, size, Pose(list(sub_origin * -1)), sub_map)
 
-    def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray:
+    def _create_from_world(self, size: int, resolution: float) -> np.ndarray:
         """
         Creates an Occupancy Costmap for the specified World.
         This map marks every position as valid that has no object above it. After
@@ -382,7 +366,7 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray:
         rays = np.dstack(np.dstack((indices_0, indices_10))).T
 
         res = np.zeros(len(rays))
-        # Using the PyBullet rayTest to check if there is an object above the position
+        # Using the World rayTest to check if there is an object above the position
         # if there is no object the position is marked as valid
         # 16383 is the maximal number of rays that can be processed in a batch
         i = 0
@@ -394,9 +378,9 @@ def _create_from_bullet(self, size: int, resolution: float) -> np.ndarray:
                     r_t = self.world.ray_test_batch(n[:, 0], n[:, 1], num_threads=0)
                 j += len(n)
                 if World.robot:
-                    shadow_robot = World.current_world.get_prospection_object_from_object(World.robot)
+                    shadow_robot = World.current_world.get_prospection_object_for_object(World.robot)
                     attached_objs = World.robot.attachments.keys()
-                    attached_objs_shadow_id = [World.current_world.get_prospection_object_from_object(x).id for x
+                    attached_objs_shadow_id = [World.current_world.get_prospection_object_for_object(x).id for x
                                                in
                                                attached_objs]
                     res[i:j] = [
@@ -520,7 +504,7 @@ def _create_images(self) -> List[np.ndarray]:
 
     def _depth_buffer_to_meter(self, buffer: np.ndarray) -> np.ndarray:
         """
-        Converts the depth images generated by PyBullet to represent
+        Converts the depth images generated by the World to represent
         each position in metre.
 
         :return: The depth image in metre
@@ -724,13 +708,13 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox:
 
         :return: Two points in world coordinate space, which span a rectangle
         """
-        prospection_object = World.current_world.get_prospection_object_from_object(self.object)
+        prospection_object = World.current_world.get_prospection_object_for_object(self.object)
         with UseProspectionWorld():
             prospection_object.set_orientation(Pose(orientation=[0, 0, 0, 1]))
             link_pose_trans = self.link.transform
             inverse_trans = link_pose_trans.invert()
             prospection_object.set_orientation(inverse_trans.to_pose())
-            return self.link.get_aabb()
+            return self.link.get_axis_aligned_bounding_box()
 
 
 cmap = colors.ListedColormap(['white', 'black', 'green', 'red', 'blue'])
@@ -747,7 +731,7 @@ def plot_grid(data: np.ndarray) -> None:
     fig, ax = plt.subplots()
     ax.imshow(data, cmap=cmap)
     # draw gridlines
-    # ax.grid(which='major', axis='both', linestyle='-', color='k', linewidth=1)
+    # ax.grid(which='major', axis='both', linestyle='-', rgba_color='k', linewidth=1)
     ax.set_xticks(np.arange(0.5, rows, 1));
     ax.set_yticks(np.arange(0.5, cols, 1));
     plt.tick_params(axis='both', labelsize=0, length=0)