Mesh and object

src/pycram/cache_manager.py

@@ -1,12 +1,17 @@
+from __future__ import annotations
+
 import glob
 import os
 import pathlib
 import shutil
 
 from typing_extensions import List, TYPE_CHECKING, Optional
 
+from .ros.logging import loginfo
+
 if TYPE_CHECKING:
     from .description import ObjectDescription
+    from .datastructures.pose import Transform
 
 
 class CacheManager:
@@ -49,8 +54,9 @@ 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,
-                                     scale_mesh: Optional[float] = None) -> str:
+                                     object_description: ObjectDescription, object_name: str,
+                                     scale_mesh: Optional[float] = None,
+                                     mesh_transform: Optional[Transform] = None) -> str:
         """
         Check if the file is already in the cache directory, if not preprocess and save in the cache.
 
@@ -60,6 +66,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool,
         :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.
+        :param mesh_transform: The transformation matrix to apply to the mesh.
         :return: The path of the cached file.
         """
         path_object = pathlib.Path(path)
@@ -73,7 +80,9 @@ 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, scale_mesh)
+            object_description.original_path = path
+            object_description.generate_description_from_file(path, object_name, extension, cache_path,
+                                                              scale_mesh, mesh_transform)
 
         return cache_path
 
@@ -90,7 +99,7 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str:
             for file in glob.glob(str(data_path), recursive=True):
                 file_path = pathlib.Path(file)
                 if file_path.name == name:
-                    print(f"Found file {name} in {file_path}")
+                    loginfo(f"Found file {name} in {file_path}")
                     return str(file_path)
 
         raise FileNotFoundError(
@@ -103,7 +112,7 @@ def create_cache_dir_if_not_exists(self):
         if not pathlib.Path(self.cache_dir).exists():
             os.mkdir(self.cache_dir)
 
-    def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool:
+    def is_cached(self, path: str, object_description: ObjectDescription) -> bool:
         """
         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
@@ -125,7 +134,7 @@ def check_with_extension(self, path: str) -> bool:
         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:
+    def check_without_extension(self, path: str, object_description: ObjectDescription) -> bool:
         """
         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.

src/pycram/costmaps.py

@@ -785,7 +785,7 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox:
             link_pose_trans = self.link.transform
             inverse_trans = link_pose_trans.invert()
             prospection_object.set_orientation(inverse_trans.to_pose())
-            return self.link.get_axis_aligned_bounding_box()
+            return self.link.get_bounding_box()
 
 
 class AlgebraicSemanticCostmap(SemanticCostmap):

src/pycram/datastructures/dataclasses.py

@@ -4,10 +4,11 @@
 from copy import deepcopy, copy
 from dataclasses import dataclass
 
-from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING
+import numpy as np
+from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Sequence
 
 from .enums import JointType, Shape, VirtualMobileBaseJointName
-from .pose import Pose, Point
+from .pose import Pose, Point, Transform
 from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable
 
 if TYPE_CHECKING:
@@ -26,6 +27,14 @@ def get_point_as_list(point: Point) -> List[float]:
     return [point.x, point.y, point.z]
 
 
+def get_list_from_points(points: List[Point]) -> List[List[float]]:
+    """
+    :param points: The points as a list of Point instances.
+    :return: The points as a list of lists of floats
+    """
+    return [get_point_as_list(point) for point in points]
+
+
 @dataclass
 class Color:
     """
@@ -88,7 +97,7 @@ def get_rgb(self) -> List[float]:
 
 
 @dataclass
-class AxisAlignedBoundingBox:
+class BoundingBox:
     """
     Dataclass for storing an axis-aligned bounding box.
     """
@@ -99,53 +108,150 @@ class AxisAlignedBoundingBox:
     max_y: float
     max_z: float
 
-    @classmethod
-    def from_min_max(cls, min_point: List[float], max_point: List[float]):
+    def get_axis_aligned_corners(self) -> List[List[float]]:
         """
-        Set the axis-aligned bounding box from a minimum and maximum point.
+        :return: The points of the bounding box as a list of lists of floats.
+        """
+        return [[self.min_x, self.min_y, self.min_z],
+                [self.min_x, self.min_y, self.max_z],
+                [self.min_x, self.max_y, self.min_z],
+                [self.min_x, self.max_y, self.max_z],
+                [self.max_x, self.min_y, self.min_z],
+                [self.max_x, self.min_y, self.max_z],
+                [self.max_x, self.max_y, self.min_z],
+                [self.max_x, self.max_y, self.max_z]]
 
-        :param min_point: The minimum point
-        :param max_point: The maximum point
+    def get_transformed_min_max_points(self, transform: Transform) -> Tuple[Point, Point]:
         """
-        return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2])
+        Apply a transformation to the bounding box and return the transformed minimum and maximum points.
+
+        :param transform: The transformation to apply
+        :return: The transformed minimum and maximum points
+        """
+        transformed_points = transform.apply_transform_to_array_of_points(np.array(self.get_min_max()))
+        min_p = [min(transformed_points[:, i]) for i in range(3)]
+        max_p = [max(transformed_points[:, i]) for i in range(3)]
+        return Point(*min_p), Point(*max_p)
 
     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 minimum and maximum points of the bounding box
         """
         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 minimum point of the bounding box
         """
         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 maximum point of the bounding box
         """
         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 minimum and maximum points of the bounding box as lists of floats
         """
         return self.get_min(), self.get_max()
 
     def get_min(self) -> List[float]:
         """
-        :return: The minimum point of the axis-aligned bounding box
+        :return: The minimum point of the axis-aligned bounding box as a list of floats
         """
         return [self.min_x, self.min_y, self.min_z]
 
     def get_max(self) -> List[float]:
         """
-        :return: The maximum point of the axis-aligned bounding box
+        :return: The maximum point of the axis-aligned bounding box as a list of floats
         """
         return [self.max_x, self.max_y, self.max_z]
 
 
+@dataclass
+class AxisAlignedBoundingBox(BoundingBox):
+
+    def get_corners(self) -> List[List[float]]:
+        """
+        :return: The points of the axis-aligned bounding box.
+        """
+        return self.get_axis_aligned_corners()
+
+    def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox':
+        """
+        Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box.
+
+        :param transform: The transformation to apply
+        :return: The transformed axis-aligned bounding box
+        """
+        transformed_points = transform.apply_transform_to_array_of_points(np.array(self.get_min_max()))
+        min_p = [min(transformed_points[:, i]) for i in range(3)]
+        max_p = [max(transformed_points[:, i]) for i in range(3)]
+        return AxisAlignedBoundingBox.from_min_max(min_p, max_p)
+
+    @classmethod
+    def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float]):
+        """
+        Set the axis-aligned bounding box from a minimum and maximum point.
+
+        :param min_point: The minimum point
+        :param max_point: The maximum point
+        """
+        return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2])
+
+
+@dataclass
+class RotatedBoundingBox(BoundingBox):
+    """
+    Dataclass for storing a rotated bounding box.
+    """
+
+    def __init__(self, min_x: float, min_y: float, min_z: float, max_x: float, max_y: float, max_z: float,
+                 transform: Transform, points: Optional[List[Point]] = None):
+        self.min_x, self.min_y, self.min_z = min_x, min_y, min_z
+        self.max_x, self.max_y, self.max_z = max_x, max_y, max_z
+        self.transform: Transform = transform
+        self._points_list: Optional[List[List[float]]] = get_list_from_points(points) if points is not None else None
+
+    @classmethod
+    def from_min_max_and_transform(cls, min_point: Sequence[float], max_point: Sequence[float], transform: Transform):
+        """
+        Set the rotated bounding box from a minimum, maximum point, and a transformation.
+
+        :param min_point: The minimum point
+        :param max_point: The maximum point
+        :param transform: The transformation
+        """
+        return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2], transform)
+
+    @classmethod
+    def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBoundingBox,
+                                       transform: Transform) -> 'RotatedBoundingBox':
+        """
+        Set the rotated bounding box from an axis-aligned bounding box and a transformation.
+
+        :param axis_aligned_bounding_box: The axis-aligned bounding box.
+        :param transform: The transformation.
+        """
+        return cls(axis_aligned_bounding_box.min_x, axis_aligned_bounding_box.min_y, axis_aligned_bounding_box.min_z,
+                   axis_aligned_bounding_box.max_x, axis_aligned_bounding_box.max_y, axis_aligned_bounding_box.max_z,
+                   transform)
+
+    def get_corners(self, transform: Optional[Transform] = None) -> List[List[float]]:
+        """
+        :param transform: The transformation to apply to the points, if None the stored transformation is used.
+        :return: The points of the rotated bounding box.
+        """
+        if (self._points_list is None) or (transform is not None):
+            if transform is not None:
+                self.transform = transform
+            points_array = np.array(self.get_axis_aligned_corners())
+            self._points_list = self.transform.apply_transform_to_array_of_points(points_array).tolist()
+        return self._points_list
+
+
 @dataclass
 class CollisionCallbacks:
     """
@@ -196,6 +302,13 @@ def visual_geometry_type(self) -> Shape:
         """
         pass
 
+    @property
+    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of the visual shape.
+        """
+        raise NotImplementedError
+
 
 @dataclass
 class BoxVisualShape(VisualShape):
@@ -215,6 +328,14 @@ def visual_geometry_type(self) -> Shape:
     def size(self) -> List[float]:
         return self.half_extents
 
+    @property
+    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of the box visual shape.
+        """
+        return AxisAlignedBoundingBox(-self.half_extents[0], -self.half_extents[1], -self.half_extents[2],
+                                      self.half_extents[0], self.half_extents[1], self.half_extents[2])
+
 
 @dataclass
 class SphereVisualShape(VisualShape):
@@ -230,6 +351,13 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.SPHERE
 
+    @property
+    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of the sphere visual shape.
+        """
+        return AxisAlignedBoundingBox(-self.radius, -self.radius, -self.radius, self.radius, self.radius, self.radius)
+
 
 @dataclass
 class CapsuleVisualShape(VisualShape):
@@ -246,6 +374,14 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.CAPSULE
 
+    @property
+    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of the capsule visual shape.
+        """
+        return AxisAlignedBoundingBox(-self.radius, -self.radius, -self.length / 2,
+                                      self.radius, self.radius, self.length / 2)
+
 
 @dataclass
 class CylinderVisualShape(CapsuleVisualShape):
@@ -490,6 +626,7 @@ 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.

src/pycram/datastructures/pose.py

@@ -380,8 +380,8 @@ def apply_transform_to_array_of_points(self, points: np.ndarray) -> np.ndarray:
         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
+        transformed_points = homogeneous_transform @ homogenous_points
+        return transformed_points[:3, :].T
 
     def get_homogeneous_matrix(self) -> np.ndarray:
         """