Mesh and object

src/pycram/cache_manager.py

@@ -3,10 +3,12 @@
 import pathlib
 import shutil
 
+import numpy as np
 from typing_extensions import List, TYPE_CHECKING, Optional
 
 if TYPE_CHECKING:
     from .description import ObjectDescription
+    from .datastructures.pose import Transform
 
 
 class CacheManager:
@@ -50,7 +52,8 @@ def delete_cache_dir(self):
 
     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:
+                                     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 +63,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 +77,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
 

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:
@@ -88,7 +89,7 @@ def get_rgb(self) -> List[float]:
 
 
 @dataclass
-class AxisAlignedBoundingBox:
+class BoundingBox:
     """
     Dataclass for storing an axis-aligned bounding box.
     """
@@ -99,15 +100,24 @@ class AxisAlignedBoundingBox:
     max_y: float
     max_z: float
 
-    @classmethod
-    def from_min_max(cls, min_point: List[float], max_point: List[float]):
+    def get_points_list(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 [[point.x, point.y, point.z] for point in self.get_points()]
 
-        :param min_point: The minimum point
-        :param max_point: The maximum point
+    def get_points(self) -> List[Point]:
         """
-        return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2])
+        :return: The points of the bounding box as a list of Point instances.
+        """
+        return [Point(self.min_x, self.min_y, self.min_z),
+                Point(self.min_x, self.min_y, self.max_z),
+                Point(self.min_x, self.max_y, self.min_z),
+                Point(self.min_x, self.max_y, self.max_z),
+                Point(self.max_x, self.min_y, self.min_z),
+                Point(self.max_x, self.min_y, self.max_z),
+                Point(self.max_x, self.max_y, self.min_z),
+                Point(self.max_x, self.max_y, self.max_z)]
 
     def get_min_max_points(self) -> Tuple[Point, Point]:
         """
@@ -146,6 +156,96 @@ def get_max(self) -> List[float]:
         return [self.max_x, self.max_y, self.max_z]
 
 
+@dataclass
+class AxisAlignedBoundingBox(BoundingBox):
+
+    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: Optional[List[Point]] = points
+
+    @classmethod
+    def from_min_max(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_points_list(self) -> List[List[float]]:
+        """
+        :return: The points of the rotated bounding box as a list of lists of floats.
+        """
+        return [[point.x, point.y, point.z] for point in self.get_points()]
+
+    def get_points(self, transform: Optional[Transform] = None) -> List[Point]:
+        """
+        :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 is None) or (transform is not None):
+            if transform is not None:
+                self.transform = transform
+            points_array = np.array([[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]])
+            transformed_points = self.transform.apply_transform_to_array_of_points(points_array).tolist()
+            self._points = [Point(*point) for point in transformed_points]
+        return self._points
+
+
 @dataclass
 class CollisionCallbacks:
     """
@@ -196,6 +296,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 +322,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 +345,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 +368,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 +620,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

@@ -379,8 +379,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:
         """