Merge pull request sunava#206 from AbdelrhmanBassiouny/mesh_and_object

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/datastructures/dataclasses.py

@@ -4,10 +4,12 @@
 from copy import deepcopy, copy
 from dataclasses import dataclass, field
 
-from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING
+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
-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 +90,7 @@ def get_rgb(self) -> List[float]:
 
 
 @dataclass
-class AxisAlignedBoundingBox:
+class BoundingBox:
     """
     Dataclass for storing an axis-aligned bounding box.
     """
@@ -99,15 +101,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 list(filter(get_point_as_list, 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]:
         """
@@ -158,6 +169,111 @@ def depth(self) -> float:
         return self.max_y - self.min_y
 
 
+@dataclass
+class AxisAlignedBoundingBox(BoundingBox):
+
+    @classmethod
+    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox':
+        """
+        Set the axis-aligned bounding box from multiple axis-aligned bounding boxes.
+
+        :param bounding_boxes: The list of axis-aligned bounding boxes.
+        """
+        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':
+        """
+        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:
     """
@@ -208,6 +324,12 @@ def visual_geometry_type(self) -> Shape:
         """
         pass
 
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of the visual shape.
+        """
+        raise NotImplementedError
+
 
 @dataclass
 class BoxVisualShape(VisualShape):
@@ -227,6 +349,13 @@ def visual_geometry_type(self) -> Shape:
     def size(self) -> List[float]:
         return self.half_extents
 
+    def get_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):
@@ -242,6 +371,12 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.SPHERE
 
+    def get_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):
@@ -258,6 +393,13 @@ def shape_data(self) -> Dict[str, float]:
     def visual_geometry_type(self) -> Shape:
         return Shape.CAPSULE
 
+    def get_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):
@@ -285,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):
@@ -301,6 +453,10 @@ def visual_geometry_type(self) -> Shape:
         return Shape.PLANE
 
 
+VisualShapeUnion = Union[BoxVisualShape, SphereVisualShape, CapsuleVisualShape,
+                         CylinderVisualShape, MeshVisualShape, PlaneVisualShape]
+
+
 @dataclass
 class State(ABC):
     """
@@ -503,6 +659,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.
@@ -718,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/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:
         """