[MeshAndObject] Added convex hull.

src/pycram/datastructures/world.py

@@ -9,6 +9,7 @@
 
 import numpy as np
 from geometry_msgs.msg import Point
+from trimesh.parent import Geometry3D
 from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type
 
 from ..cache_manager import CacheManager
@@ -126,6 +127,24 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca
 
         self.on_add_object_callbacks: List[Callable[[Object], None]] = []
 
+    def get_object_convex_hull(self, obj: Object) -> Geometry3D:
+        """
+        Get the convex hull of an object.
+
+        :param obj: The pycram object.
+        :return: The convex hull of the object as a list of Points.
+        """
+        raise NotImplementedError
+
+    def get_link_convex_hull(self, link: Link) -> Geometry3D:
+        """
+        Get the convex hull of a link of an articulated object.
+
+        :param link: The link as a AbstractLink object.
+        :return: The convex hull of the link as a list of Points.
+        """
+        raise NotImplementedError
+
     def add_callback_on_add_object(self, callback: Callable[[Object], None]) -> None:
         """
         Add a callback that is called when an object is added to the world.

src/pycram/description.py

@@ -5,6 +5,8 @@
 import pathlib
 from abc import ABC, abstractmethod
 
+from trimesh.parent import Geometry3D
+
 from .ros.data_types import Time
 import trimesh
 from geometry_msgs.msg import Point, Quaternion
@@ -247,6 +249,27 @@ def get_bounding_box(self, rotated: bool = False) -> Union[AxisAlignedBoundingBo
                 else:
                     return bounding_box.get_transformed_box(self.transform)
 
+    def get_convex_hull(self) -> Geometry3D:
+        """
+        :return: The convex hull of the link geometry.
+        """
+        try:
+            return self.world.get_link_convex_hull(self)
+        except NotImplementedError:
+            if isinstance(self.geometry, MeshVisualShape):
+                mesh_path = self.get_mesh_path()
+                mesh = trimesh.load(mesh_path)
+                return trimesh.convex.convex_hull(mesh).apply_transform(self.transform.get_homogeneous_matrix())
+            else:
+                raise LinkGeometryHasNoMesh(self.name, type(self.geometry).__name__)
+
+    def _plot_convex_hull(self):
+        """
+        Plot the convex hull of the link geometry.
+        """
+        hull = self.get_convex_hull()
+        hull.show()
+
     def get_mesh_path(self) -> str:
         """
         :return: The path of the mesh file of this link if the geometry is a mesh.

src/pycram/world_concepts/world_object.py

@@ -7,6 +7,7 @@
 import numpy as np
 from deprecated import deprecated
 from geometry_msgs.msg import Point, Quaternion
+from trimesh.parent import Geometry3D
 from typing_extensions import Type, Optional, Dict, Tuple, List, Union
 
 from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState,
@@ -1375,7 +1376,7 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
         :return: The axis aligned bounding box of this object.
         """
         if self.has_one_link:
-            return self.links[self.description.get_root()].get_bounding_box()
+            return self.root_link.get_bounding_box()
         else:
             return self.world.get_object_axis_aligned_bounding_box(self)
 
@@ -1386,10 +1387,21 @@ def get_rotated_bounding_box(self) -> RotatedBoundingBox:
         :return: The rotated bounding box of this object.
         """
         if self.has_one_link:
-            return self.links[self.description.get_root()].get_bounding_box(rotated=True)
+            return self.root_link.get_bounding_box(rotated=True)
         else:
             return self.world.get_object_rotated_bounding_box(self)
 
+    def get_convex_hull(self) -> Geometry3D:
+        """
+        Return the convex hull of this object.
+
+        :return: The convex hull of this object.
+        """
+        if self.has_one_link:
+            return self.root_link.get_convex_hull()
+        else:
+            return self.world.get_object_convex_hull(self)
+
     def get_base_origin(self) -> Pose:
         """
         Return the origin of the base/bottom of this object.

test/test_links.py

@@ -1,6 +1,7 @@
 import numpy as np
 from matplotlib import pyplot as plt
 from tf.transformations import quaternion_from_euler
+from typing_extensions import List
 
 from bullet_world_testcase import BulletWorldTestCase
 from pycram.datastructures.dataclasses import Color
@@ -9,6 +10,16 @@
 
 class TestLinks(BulletWorldTestCase):
 
+    def test_get_convex_hull(self):
+        self.milk.set_orientation(quaternion_from_euler(0, np.pi/4, 0))
+        hull = self.milk.root_link.get_convex_hull()
+        self.assertIsNotNone(hull)
+        self.assertTrue(len(hull.vertices) > 0)
+        self.assertTrue(len(hull.faces) > 0)
+        plot = False
+        if plot:
+            self.plot_3d_points([hull.vertices])
+
     def test_rotated_bounding_box(self):
         self.milk.set_pose(Pose([1, 1, 1], quaternion_from_euler(np.pi/4, 0, 0).tolist()))
         aabb = self.milk.get_axis_aligned_bounding_box()
@@ -17,20 +28,25 @@ def test_rotated_bounding_box(self):
         rot_points = np.array(rbb.get_points_list())
         plot = False
         if plot:
-            fig = plt.figure()
-            ax = fig.add_subplot(projection='3d')
+            self.plot_3d_points([aabb_points, rot_points])
+
+    @staticmethod
+    def plot_3d_points(list_of_points: List[np.ndarray]):
+        fig = plt.figure()
+        ax = fig.add_subplot(projection='3d')
 
-            ax.scatter(rot_points[:, 0], rot_points[:, 1], rot_points[:, 2], c='r', marker='o')
-            ax.scatter(aabb_points[:, 0], aabb_points[:, 1], aabb_points[:, 2], c='b', marker='o')
+        for points in list_of_points:
+            color = np.random.rand(3,)
+            ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=color, marker='o')
 
-            ax.set_xlabel('X Label')
-            ax.set_ylabel('Y Label')
-            ax.set_zlabel('Z Label')
-            plt.xlim(0, 2)
-            plt.ylim(0, 2)
-            ax.set_zlim(0, 2)
+        ax.set_xlabel('X Label')
+        ax.set_ylabel('Y Label')
+        ax.set_zlabel('Z Label')
+        plt.xlim(0, 2)
+        plt.ylim(0, 2)
+        ax.set_zlim(0, 2)
 
-            plt.show()
+        plt.show()
 
     def test_add_constraint(self):
         milk_link = self.milk.root_link