[MeshAndObject] performed review changes and added ability to get bou…

…nding box for multi geometry links.

src/pycram/costmaps.py

@@ -783,7 +783,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_bounding_box()
+            return self.link.get_axis_aligned_bounding_box()
 
 
 class AlgebraicSemanticCostmap(SemanticCostmap):

src/pycram/datastructures/dataclasses.py

@@ -5,6 +5,7 @@
 from dataclasses import dataclass, field
 
 import numpy as np
+import trimesh
 from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Sequence
 
 from .enums import JointType, Shape, VirtualMobileBaseJointName
@@ -27,14 +28,6 @@ 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:
     """
@@ -108,64 +101,58 @@ class BoundingBox:
     max_y: float
     max_z: float
 
-    def get_axis_aligned_corners(self) -> List[List[float]]:
+    def get_points_list(self) -> List[List[float]]:
         """
         :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]]
+        return list(filter(get_point_as_list, self.get_points()))
 
-    def get_transformed_min_max_points(self, transform: Transform) -> Tuple[Point, Point]:
+    def get_points(self) -> List[Point]:
         """
-        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
+        :return: The points of the bounding box as a list of Point instances.
         """
-        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)
+        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]:
         """
-        :return: The minimum and maximum points of the bounding box
+        :return: The axis-aligned bounding box as a tuple of minimum and maximum points
         """
         return self.get_min_point(), self.get_max_point()
 
     def get_min_point(self) -> Point:
         """
-        :return: The minimum point of the bounding box
+        :return: The axis-aligned bounding box as a minimum point
         """
         return Point(self.min_x, self.min_y, self.min_z)
 
     def get_max_point(self) -> Point:
         """
-        :return: The maximum point of the bounding box
+        :return: The axis-aligned bounding box as a maximum point
         """
         return Point(self.max_x, self.max_y, self.max_z)
 
     def get_min_max(self) -> Tuple[List[float], List[float]]:
         """
-        :return: The minimum and maximum points of the bounding box as lists of floats
+        :return: The axis-aligned bounding box as a tuple of minimum and maximum points
         """
         return self.get_min(), self.get_max()
 
     def get_min(self) -> List[float]:
         """
-        :return: The minimum point of the axis-aligned bounding box as a list of floats
+        :return: The minimum point of the axis-aligned bounding box
         """
         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 as a list of floats
+        :return: The maximum point of the axis-aligned bounding box
         """
         return [self.max_x, self.max_y, self.max_z]
 
@@ -185,11 +172,20 @@ def depth(self) -> float:
 @dataclass
 class AxisAlignedBoundingBox(BoundingBox):
 
-    def get_corners(self) -> List[List[float]]:
+    @classmethod
+    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox':
         """
-        :return: The points of the axis-aligned bounding box.
+        Set the axis-aligned bounding box from multiple axis-aligned bounding boxes.
+
+        :param bounding_boxes: The list of axis-aligned bounding boxes.
         """
-        return self.get_axis_aligned_corners()
+        min_x = min([box.min_x for box in bounding_boxes])
+        min_y = min([box.min_y for box in bounding_boxes])
+        min_z = min([box.min_z for box in bounding_boxes])
+        max_x = max([box.max_x for box in bounding_boxes])
+        max_y = max([box.max_y for box in bounding_boxes])
+        max_z = max([box.max_z for box in bounding_boxes])
+        return cls(min_x, min_y, min_z, max_x, max_y, max_z)
 
     def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox':
         """
@@ -225,10 +221,10 @@ def __init__(self, min_x: float, min_y: float, min_z: float, max_x: float, max_y
         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
+        self._points: Optional[List[Point]] = points
 
     @classmethod
-    def from_min_max_and_transform(cls, min_point: Sequence[float], max_point: Sequence[float], transform: Transform):
+    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.
 
@@ -251,17 +247,31 @@ def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBo
                    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]]:
+    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_list is None) or (transform is not None):
+        if (self._points 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
+            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
@@ -314,8 +324,7 @@ def visual_geometry_type(self) -> Shape:
         """
         pass
 
-    @property
-    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
         """
         :return: The axis-aligned bounding box of the visual shape.
         """
@@ -340,8 +349,7 @@ def visual_geometry_type(self) -> Shape:
     def size(self) -> List[float]:
         return self.half_extents
 
-    @property
-    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
         """
         :return: The axis-aligned bounding box of the box visual shape.
         """
@@ -363,8 +371,7 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.SPHERE
 
-    @property
-    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
         """
         :return: The axis-aligned bounding box of the sphere visual shape.
         """
@@ -386,8 +393,7 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.CAPSULE
 
-    @property
-    def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
         """
         :return: The axis-aligned bounding box of the capsule visual shape.
         """
@@ -421,6 +427,16 @@ def shape_data(self) -> Dict[str, Union[List[float], str]]:
     def visual_geometry_type(self) -> Shape:
         return Shape.MESH
 
+    def get_axis_aligned_bounding_box(self, file_path: Optional[str] = None) -> AxisAlignedBoundingBox:
+        """
+        :param file_path: An alternative file path.
+        :return: The axis-aligned bounding box of the mesh visual shape.
+        """
+        mesh_file_path = file_path if file_path is not None else self.file_name
+        mesh = trimesh.load(mesh_file_path)
+        min_bound, max_bound = mesh.bounds
+        return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound)
+
 
 @dataclass
 class PlaneVisualShape(VisualShape):
@@ -437,6 +453,10 @@ def visual_geometry_type(self) -> Shape:
         return Shape.PLANE
 
 
+VisualShapeUnion = Union[BoxVisualShape, SphereVisualShape, CapsuleVisualShape,
+                         CylinderVisualShape, MeshVisualShape, PlaneVisualShape]
+
+
 @dataclass
 class State(ABC):
     """
@@ -855,4 +875,4 @@ class ReasoningResult:
     Result of a reasoning result of knowledge source
     """
     success: bool
-    reasoned_parameter: Dict[str, Any] = field(default_factory=dict)
+    reasoned_parameter: Dict[str, Any] = field(default_factory=dict)

src/pycram/description.py

@@ -56,7 +56,7 @@ def __init__(self, parsed_link_description: Any):
 
     @property
     @abstractmethod
-    def geometry(self) -> Union[VisualShape, None]:
+    def geometry(self) -> Union[List[VisualShape], VisualShape, None]:
         """
         The geometry type of the collision element of this link.
         """
@@ -222,32 +222,44 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object):
         self._current_pose: Optional[Pose] = None
         self.update_pose()
 
-    def get_bounding_box(self, rotated: bool = False) -> Union[AxisAlignedBoundingBox, RotatedBoundingBox]:
+    def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox:
         """
-        :param rotated: If True, return the rotated bounding box, otherwise the axis-aligned bounding box.
-        :return: The axis-aligned or rotated bounding box of a link. First try to get it from the simulator, if not,
+        :param transform_to_link_pose: If True, return the bounding box transformed to the link pose.
+        :return: The axis-aligned bounding box of a link. First try to get it from the simulator, if not,
          then calculate it depending on the type of the link geometry.
         """
         try:
-            if rotated:
-                return self.world.get_link_rotated_bounding_box(self)
-            else:
-                return self.world.get_link_axis_aligned_bounding_box(self)
+            return self.world.get_link_axis_aligned_bounding_box(self)
         except NotImplementedError:
-            if isinstance(self.geometry, MeshVisualShape):
-                mesh_path = self.get_mesh_path()
-                mesh = trimesh.load(mesh_path)
-                min_bound, max_bound = mesh.bounds
-                if rotated:
-                    return RotatedBoundingBox.from_min_max_and_transform(min_bound, max_bound, self.transform)
-                else:
-                    return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound).get_transformed_box(self.transform)
+            bounding_box = self.get_axis_aligned_bounding_box_from_geometry()
+            if transform_to_link_pose:
+                return bounding_box.get_transformed_box(self.transform)
             else:
-                bounding_box = self.geometry.axis_aligned_bounding_box
-                if rotated:
-                    return RotatedBoundingBox.from_axis_aligned_bounding_box(bounding_box, self.transform)
-                else:
-                    return bounding_box.get_transformed_box(self.transform)
+                return bounding_box
+
+    def get_rotated_bounding_box(self) -> RotatedBoundingBox:
+        """
+        :return: The rotated bounding box of a link. First try to get it from the simulator, if not,
+         then calculate it depending on the type of the link geometry.
+        """
+        try:
+            return self.world.get_link_rotated_bounding_box(self)
+        except NotImplementedError:
+            return RotatedBoundingBox.from_axis_aligned_bounding_box(self.get_axis_aligned_bounding_box(),
+                                                                     self.transform)
+
+    def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox:
+        if isinstance(self.geometry, List):
+            all_boxes = [geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom))
+                         if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box()
+                         for geom in self.geometry
+                         ]
+            bounding_box = AxisAlignedBoundingBox.from_multiple_bounding_boxes(all_boxes)
+        else:
+            geom = self.geometry
+            bounding_box = geom.get_axis_aligned_bounding_box(self.get_mesh_path(geom)) \
+                if isinstance(geom, MeshVisualShape) else geom.get_axis_aligned_bounding_box()
+        return bounding_box
 
     def get_convex_hull(self) -> Geometry3D:
         """
@@ -257,7 +269,7 @@ def get_convex_hull(self) -> Geometry3D:
             return self.world.get_link_convex_hull(self)
         except NotImplementedError:
             if isinstance(self.geometry, MeshVisualShape):
-                mesh_path = self.get_mesh_path()
+                mesh_path = self.get_mesh_path(self.geometry)
                 mesh = trimesh.load(mesh_path)
                 return trimesh.convex.convex_hull(mesh).apply_transform(self.transform.get_homogeneous_matrix())
             else:
@@ -270,25 +282,26 @@ def _plot_convex_hull(self):
         hull = self.get_convex_hull()
         hull.show()
 
-    def get_mesh_path(self) -> str:
+    def get_mesh_path(self, geometry: MeshVisualShape) -> str:
         """
+        :param geometry: The geometry for which the mesh path should be returned.
         :return: The path of the mesh file of this link if the geometry is a mesh.
         """
-        mesh_filename = self.get_mesh_filename()
+        mesh_filename = self.get_mesh_filename(geometry)
         return self.world.cache_manager.look_for_file_in_data_dir(pathlib.Path(mesh_filename))
 
-    def get_mesh_filename(self) -> str:
+    def get_mesh_filename(self, geometry: MeshVisualShape) -> str:
         """
         :return: The mesh file name of this link if the geometry is a mesh, otherwise raise a LinkGeometryHasNoMesh.
         :raises LinkHasNoGeometry: If the link has no geometry.
         :raises LinkGeometryHasNoMesh: If the geometry is not a mesh.
         """
-        if self.geometry is None:
+        if geometry is None:
             raise LinkHasNoGeometry(self.name)
-        if isinstance(self.geometry, MeshVisualShape):
-            return self.geometry.file_name
+        if isinstance(geometry, MeshVisualShape):
+            return geometry.file_name
         else:
-            raise LinkGeometryHasNoMesh(self.name, type(self.geometry).__name__)
+            raise LinkGeometryHasNoMesh(self.name, type(geometry).__name__)
 
     def set_pose(self, pose: Pose) -> None:
         """