From 394262666aae7daa9956eb451ea54db78db71a2e Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Tue, 17 Dec 2024 19:57:34 +0100
Subject: [PATCH 01/13] [MergeBoundingBoxes] used random events to merge boxes,
 need to convert to mesh.

---
 src/pycram/datastructures/dataclasses.py | 148 +++++++++++++++++++++--
 1 file changed, 141 insertions(+), 7 deletions(-)

diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index 7a63668f5..84b8f6422 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -7,6 +7,12 @@
 
 import numpy as np
 import trimesh
+from random_events.variable import Continuous
+from random_events.interval import closed, Interval
+from random_events.product_algebra import SimpleEvent
+from pyvista import PolyData
+import pyvista as pv
+import plotly.graph_objects as go
 from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Sequence
 
 from .enums import JointType, Shape, VirtualMobileBaseJointName
@@ -103,11 +109,24 @@ class BoundingBox:
     max_y: float
     max_z: float
 
+    @property
+    def origin(self) -> List[float]:
+        return [(self.min_x + self.max_x) / 2, (self.min_y + self.max_y) / 2, (self.min_z + self.max_z) / 2]
+
+    @property
+    def base_origin(self) -> List[float]:
+        center = self.origin
+        return [center[0], center[1], self.min_z]
+
+    @property
+    def origin_point(self) -> Point:
+        return Point(*self.origin)
+
     def get_points_list(self) -> List[List[float]]:
         """
         :return: The points of the bounding box as a list of lists of floats.
         """
-        return list(filter(get_point_as_list, self.get_points()))
+        return [[point.x, point.y, point.z] for point in self.get_points()]
 
     def get_points(self) -> List[Point]:
         """
@@ -201,6 +220,127 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo
         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)
 
+    @staticmethod
+    def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoundingBox],
+                                                save_mesh_to: Optional[str] = None,
+                                                use_random_events: bool = True,
+                                                plot: bool = False) -> Union[PolyData, Interval]:
+        """
+        Merge multiple axis-aligned bounding boxes into a single mesh.
+
+        :param bounding_boxes: The list of axis-aligned bounding boxes.
+        :param save_mesh_to: The file path to save the mesh to.
+        :param use_random_events: If True, use random events to compute the new shape, otherwise use pyvista.
+        :param plot: If True, plot the mesh.
+        :return: The mesh of the merged bounding boxes.
+        """
+        if use_random_events:
+            x = Continuous("x")
+            y = Continuous("y")
+            z = Continuous("z")
+
+            all_intervals = [(box.get_min(), box.get_max()) for box in bounding_boxes]
+            interval = None
+            for min_point, max_point in all_intervals:
+                event = SimpleEvent({x: closed(min_point[0], max_point[0]), y: closed(min_point[1], max_point[1]),
+                                     z: closed(min_point[2], max_point[2])}).as_composite_set()
+                if interval is None:
+                    interval = event
+                else:
+                    interval = interval.union_with(event) - interval.intersection_with(event)
+            simplified_intervals = interval.make_disjoint()
+            if plot:
+                fig = go.Figure(simplified_intervals.plot(), interval.plotly_layout())
+                fig.update_layout(title="Merged Bounding Boxes")
+                fig.show()
+            return simplified_intervals
+        else:
+            all_points = [point for box in bounding_boxes for point in box.get_points_list()]
+            cloud = pv.PolyData(all_points)
+            volume = cloud.delaunay_3d(alpha=2.)
+            shell = volume.extract_geometry()
+            if plot:
+                shell.plot()
+            if save_mesh_to is not None:
+                shell.save(save_mesh_to)
+            return shell
+
+    def merge_with_other_box(self, other_box: 'AxisAlignedBoundingBox',
+                             get_mesh: bool = False,
+                             save_mesh_to: Optional[str] = None) -> Union[List[List[float]], trimesh.parent.Geometry3D]:
+        """
+        Merge the axis-aligned bounding box with another axis-aligned bounding box.
+
+        :param other_box: The other axis-aligned bounding box.
+        :param get_mesh: If True, return the mesh of the merged bounding boxes.
+        :param save_mesh_to: The file path to save the mesh to.
+        :return: The points of the final shape as a list of lists of floats or the mesh of the merged bounding boxes.
+        """
+        intersection_points, inside_points = self.get_intersection_points_with_other_box(other_box,
+                                                                                         return_inside_points=True,
+                                                                                         as_list=True)
+        all_points = [point for box in [self, other_box] for point in box.get_points_list()
+                      if point not in inside_points]
+        all_points.extend(intersection_points)
+
+        if get_mesh:
+            # create a mesh from the points
+            mesh = trimesh.Trimesh(vertices=np.array(all_points), faces=trimesh.convex.convex_hull(all_points))
+            if save_mesh_to is not None:
+                mesh.export(save_mesh_to)
+            return mesh
+        else:
+            return all_points
+
+    def get_intersection_points_with_other_box(self, other_box: 'AxisAlignedBoundingBox',
+                                               return_inside_points: bool = False,
+                                               as_list: bool = False) -> Union[Tuple[List, List], List]:
+        """
+        Get the intersection points with another axis-aligned bounding box.
+
+        :param other_box: The other axis-aligned bounding box.
+        :param return_inside_points: If True, return the points that are inside the bounding boxes.
+        :param as_list: If True, return the points as a list of lists of floats.
+        :return: The intersection points as a list of points.
+        """
+        intersection_box = self.get_intersection_box_with_other_box(other_box)
+        intersection_points = []
+        inside_points = []
+        for point in intersection_box.get_points():
+            if not self.is_point_inside(point) and not other_box.is_point_inside(point):
+                intersection_points.append(get_point_as_list(point) if as_list else point)
+            elif return_inside_points:
+                inside_points.append(get_point_as_list(point) if as_list else point)
+        if return_inside_points:
+            return intersection_points, inside_points
+        return intersection_points
+
+    def get_intersection_box_with_other_box(self, other_box: 'AxisAlignedBoundingBox') -> 'AxisAlignedBoundingBox':
+        """
+        Get the intersection box with another axis-aligned bounding box.
+
+        :param other_box: The other axis-aligned bounding box.
+        :return: The intersection box.
+        """
+        min_x = max(self.min_x, other_box.min_x)
+        min_y = max(self.min_y, other_box.min_y)
+        min_z = max(self.min_z, other_box.min_z)
+        max_x = min(self.max_x, other_box.max_x)
+        max_y = min(self.max_y, other_box.max_y)
+        max_z = min(self.max_z, other_box.max_z)
+        return AxisAlignedBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z)
+
+    def is_point_inside(self, point: Point) -> bool:
+        """
+        Check if a point is inside the axis-aligned bounding box.
+
+        :param point: The point to check
+        :return: True if the point is inside the bounding box, False otherwise
+        """
+        return (self.min_x < point.x < self.max_x
+                and self.min_y < point.y < self.max_y
+                and self.min_z < point.z < self.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.
@@ -261,12 +401,6 @@ 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_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.

From 5148c872a5db242264b084dc3f0c9ac7c1060e81 Mon Sep 17 00:00:00 2001
From: A_Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Wed, 18 Dec 2024 00:46:27 +0100
Subject: [PATCH 02/13] [MergeBoundingBoxes] creating mesh in progress.

---
 config/world_conf.py                     |  4 +--
 src/pycram/datastructures/dataclasses.py | 38 ++++++++++++++++++++----
 2 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/config/world_conf.py b/config/world_conf.py
index 75f335d07..4b6756d7e 100644
--- a/config/world_conf.py
+++ b/config/world_conf.py
@@ -2,8 +2,8 @@
 import os
 
 from typing_extensions import Tuple, Type
-from pycram.description import ObjectDescription
-from pycram.object_descriptors.urdf import ObjectDescription as URDF
+from ..description import ObjectDescription
+from ..object_descriptors.urdf import ObjectDescription as URDF
 
 
 class WorldConfig:
diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index 84b8f6422..1464a35dc 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -224,7 +224,7 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo
     def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoundingBox],
                                                 save_mesh_to: Optional[str] = None,
                                                 use_random_events: bool = True,
-                                                plot: bool = False) -> Union[PolyData, Interval]:
+                                                plot: bool = True) -> Union[PolyData, Interval]:
         """
         Merge multiple axis-aligned bounding boxes into a single mesh.
 
@@ -240,7 +240,7 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
             z = Continuous("z")
 
             all_intervals = [(box.get_min(), box.get_max()) for box in bounding_boxes]
-            interval = None
+            event = SimpleEvent()
             for min_point, max_point in all_intervals:
                 event = SimpleEvent({x: closed(min_point[0], max_point[0]), y: closed(min_point[1], max_point[1]),
                                      z: closed(min_point[2], max_point[2])}).as_composite_set()
@@ -248,12 +248,12 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
                     interval = event
                 else:
                     interval = interval.union_with(event) - interval.intersection_with(event)
-            simplified_intervals = interval.make_disjoint()
+            vertices, faces = AxisAlignedBoundingBox.get_mesh_data_from_interval(interval)
             if plot:
-                fig = go.Figure(simplified_intervals.plot(), interval.plotly_layout())
+                fig = go.Figure(interval.plot(), interval.plotly_layout())
                 fig.update_layout(title="Merged Bounding Boxes")
                 fig.show()
-            return simplified_intervals
+            return interval
         else:
             all_points = [point for box in bounding_boxes for point in box.get_points_list()]
             cloud = pv.PolyData(all_points)
@@ -265,6 +265,34 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
                 shell.save(save_mesh_to)
             return shell
 
+    @staticmethod
+    def get_mesh_data_from_interval(interval: Interval) -> Tuple[np.ndarray, np.ndarray]:
+        # form cartesian product of all intervals
+        intervals = [value.simple_sets for _, value in sorted(self.items())]
+        simple_events = list(itertools.product(*intervals))
+        traces = []
+
+        # shortcut for the dimensions
+        x, y, z = 0, 1, 2
+
+        # for every atomic interval
+        for simple_event in simple_events:
+            # Create a 3D mesh trace for the rectangle
+            traces.append(go.Mesh3d(
+                # 8 vertices of a cube
+                x=[simple_event[x].lower, simple_event[x].lower, simple_event[x].upper, simple_event[x].upper,
+                   simple_event[x].lower, simple_event[x].lower, simple_event[x].upper, simple_event[x].upper],
+                y=[simple_event[y].lower, simple_event[y].upper, simple_event[y].upper, simple_event[y].lower,
+                   simple_event[y].lower, simple_event[y].upper, simple_event[y].upper, simple_event[y].lower],
+                z=[simple_event[z].lower, simple_event[z].lower, simple_event[z].lower, simple_event[z].lower,
+                   simple_event[z].upper, simple_event[z].upper, simple_event[z].upper, simple_event[z].upper],
+                # i, j and k give the vertices of triangles
+                i=[7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2],
+                j=[3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3],
+                k=[0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6],
+                flatshading=True
+            ))
+
     def merge_with_other_box(self, other_box: 'AxisAlignedBoundingBox',
                              get_mesh: bool = False,
                              save_mesh_to: Optional[str] = None) -> Union[List[List[float]], trimesh.parent.Geometry3D]:

From dfef7df7b8cf53cdf00f95a6135628397ff0344a Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Thu, 19 Dec 2024 17:49:19 +0100
Subject: [PATCH 03/13] [MergeDescription] added a functionality of merging two
 descriptions.

---
 src/pycram/description.py             |  27 +++-
 src/pycram/object_descriptors/urdf.py | 183 ++++++++++++++++++++++----
 test/test_description.py              |  14 ++
 3 files changed, 197 insertions(+), 27 deletions(-)

diff --git a/src/pycram/description.py b/src/pycram/description.py
index ff14c1f91..1b647cb4f 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -8,7 +8,7 @@
 import trimesh
 from geometry_msgs.msg import Point
 from trimesh.parent import Geometry3D
-from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence
+from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Sequence
 
 from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \
     MeshVisualShape, RotatedBoundingBox
@@ -57,7 +57,7 @@ class LinkDescription(EntityDescription):
     """
 
     def __init__(self, parsed_link_description: Any, mesh_dir: Optional[str] = None):
-        self.parsed_description = parsed_link_description
+        super().__init__(parsed_link_description)
         self.mesh_dir = mesh_dir
 
     @property
@@ -357,7 +357,7 @@ def all_constraint_links_belong_to_same_world(self, other: LinkState) -> bool:
         return all([link.world == other_link.world for link, other_link in zip(self.constraint_ids.keys(),
                                                                                other.constraint_ids.keys())])
 
-    def add_fixed_constraint_with_link(self, child_link: Self,
+    def add_fixed_constraint_with_link(self, child_link: Link,
                                        child_to_parent_transform: Optional[Transform] = None) -> int:
         """
         Add a fixed constraint between this link and the given link, to create attachments for example.
@@ -725,6 +725,27 @@ def add_joint(self, name: str, child: str, joint_type: JointType,
         """
         pass
 
+    @abstractmethod
+    def merge_description(self, other: ObjectDescription, parent_link: Optional[str] = None,
+                          child_link: Optional[str] = None,
+                          joint_type: JointType = JointType.FIXED,
+                          axis: Optional[Point] = None,
+                          lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                          child_pose_wrt_parent: Optional[Pose] = None) -> None:
+        """
+        Merge the description of this object with the description of the other object.
+
+        :param other: The object description to merge with this one.
+        :param parent_link: The name of the parent link of the joint connecting the two objects.
+        :param child_link: The name of the child link of the joint connecting the two objects.
+        :param joint_type: The type of the joint connecting the two objects.
+        :param axis: The axis of the joint connecting the two objects.
+        :param lower_limit: The lower limit of the joint connecting the two objects.
+        :param upper_limit: The upper limit of the joint connecting the two objects.
+        :param child_pose_wrt_parent: The pose of the child link with respect to the parent link.
+        """
+        pass
+
     def update_description_from_file(self, path: str) -> None:
         """
         Update the description of this object from the file at the given path.
diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py
index bf7c196a8..f4c97d503 100644
--- a/src/pycram/object_descriptors/urdf.py
+++ b/src/pycram/object_descriptors/urdf.py
@@ -1,25 +1,27 @@
+from __future__ import annotations
+
+import copy
 import os
 import pathlib
 import xml.etree.ElementTree as ET
 
 import numpy as np
-
-from ..ros.logging import logerr, logwarn
-from ..ros.ros_tools import create_ros_pack, ResourceNotFound, get_parameter
 from geometry_msgs.msg import Point
 from tf.transformations import quaternion_from_euler, euler_from_quaternion
-from typing_extensions import Union, List, Optional, Dict, Tuple
+from typing_extensions import Union, List, Optional, Dict, Tuple, Type
 from urdf_parser_py import urdf
 from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder,
                                  Sphere as URDF_Sphere, Mesh as URDF_Mesh)
 
+from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \
+    SphereVisualShape, MeshVisualShape
 from ..datastructures.enums import JointType
 from ..datastructures.pose import Pose
 from ..description import JointDescription as AbstractJointDescription, \
     LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription
-from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \
-    SphereVisualShape, MeshVisualShape
 from ..failures import MultiplePossibleTipLinks
+from ..ros.logging import logerr
+from ..ros.ros_tools import create_ros_pack, ResourceNotFound, get_parameter
 from ..utils import suppress_stdout_stderr
 
 
@@ -212,7 +214,7 @@ def link_map(self) -> Dict[str, LinkDescription]:
         :return: A dictionary mapping the name of a link to its description.
         """
         if self._link_map is None:
-            self._link_map = {link.name: link for link in self.links}
+            self._init_links_map()
         return self._link_map
 
     @property
@@ -221,11 +223,17 @@ def joint_map(self) -> Dict[str, JointDescription]:
         :return: A dictionary mapping the name of a joint to its description.
         """
         if self._joint_map is None:
-            self._joint_map = {joint.name: joint for joint in self.joints}
+            self._init_joints_map()
         return self._joint_map
 
+    def _init_joints_map(self) -> None:
+        self._joint_map = {joint.name: joint for joint in self.joints}
+
+    def _init_links_map(self) -> None:
+        self._link_map = {link.name: link for link in self.links}
+
     def add_joint(self, name: str, child: str, joint_type: JointType,
-                  axis: Point, parent: Optional[str] = None, origin: Optional[Pose] = None,
+                  axis: Optional[Point] = None, parent: Optional[str] = None, origin: Optional[Pose] = None,
                   lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
                   is_virtual: Optional[bool] = False) -> None:
         """
@@ -242,16 +250,135 @@ def add_joint(self, name: str, child: str, joint_type: JointType,
             axis = [axis.x, axis.y, axis.z]
         if parent is None:
             parent = self.get_root()
-        else:
-            parent = self.get_link_by_name(parent).parsed_description
-        joint = urdf.Joint(name,
-                           parent,
-                           self.get_link_by_name(child).parsed_description,
-                           JointDescription.pycram_type_map[joint_type],
-                           axis, origin, limit)
-        self.parsed_description.add_joint(joint)
         if is_virtual:
             self.virtual_joint_names.append(name)
+        else:
+            joint = urdf.Joint(name,
+                               parent,
+                               child,
+                               JointDescription.pycram_type_map[joint_type],
+                               axis, origin, limit)
+            self.add_joint_from_parsed_description(joint)
+
+    def add_joint_from_parsed_description(self, description: urdf.Joint) -> None:
+        """
+        Add a joint to the object description from a parsed URDF joint description.
+
+        :param description: The parsed URDF joint description.
+        """
+        self.parsed_description.add_joint(description)
+        if self._joints is not None:
+            self._joints.append(JointDescription(description))
+            self._joint_map[description.name] = self._joints[-1]
+
+    def add_link_from_parsed_description(self, description: urdf.Link) -> None:
+        """
+        Add a link to the object description from a parsed URDF link description.
+
+        :param description: The parsed URDF link description.
+        """
+        self.parsed_description.add_link(description)
+        if self._links is not None and len(self._links) > 0:
+            self._links.append(LinkDescription(description))
+            self._link_map[description.name] = self._links[-1]
+
+    def merge_description(self, other: ObjectDescription, parent_link: Optional[str] = None,
+                          child_link: Optional[str] = None,
+                          joint_type: JointType = JointType.FIXED,
+                          axis: Optional[Point] = None,
+                          lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                          child_pose_wrt_parent: Optional[Pose] = None) -> None:
+        other_description = other.parsed_description
+        if child_pose_wrt_parent is None:
+            child_pose_wrt_parent = Pose()
+
+        original_child_link = child_link if child_link is not None else other.get_root()
+        child_link = f"{other_description.name}_" + original_child_link
+        parent_link = parent_link if parent_link is not None else self.get_root()
+
+        # Add the links of the other description to this description
+        for link in other_description.links:
+            # Change the name of the link to avoid name conflicts
+            link = copy.deepcopy(link)
+            link.name = f'{other_description.name}_{link.name}'
+            self.add_link_from_parsed_description(link)
+
+        # Add the joints of the other description to this description
+        for joint in other_description.joints:
+            # Change the name of the joint to avoid name conflicts
+            joint = copy.deepcopy(joint)
+            joint.name = f'{other_description.name}_{joint.name}'
+            self.add_joint_from_parsed_description(joint)
+
+        # Add the joint between the parent and child link
+        self.add_joint(f'{parent_link}_{original_child_link}_joint', child_link, joint_type,
+                       axis=axis, origin=child_pose_wrt_parent, parent=parent_link,
+                       lower_limit=lower_limit, upper_limit=upper_limit)
+
+        self.write_description_to_file(self.parsed_description.to_xml_string(), self.original_path)
+
+    def merge_description_using_et(self, other: ObjectDescription, parent_link: Optional[str] = None,
+                                   child_link: Optional[str] = None,
+                                   joint_type: JointType = JointType.FIXED,
+                                   axis: Optional[Point] = None,
+                                   lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                                   child_pose_wrt_parent: Optional[Pose] = None) -> None:
+        """
+        Merge the description of another object into this object description using the element tree library.
+        parameters are the same as in :meth:`pycram.description.ObjectDescription.merge_description`.
+        """
+        other_description = other.parsed_description
+        if child_pose_wrt_parent is None:
+            child_pose_wrt_parent = Pose()
+        other_et = ET.fromstring(other_description.to_xml_string())
+        my_et = ET.fromstring(self.parsed_description.to_xml_string())
+        # Add the links of the other description to this description
+        for link in other_et.findall('link'):
+            # Change the name of the link to avoid name conflicts
+            link.set('name', f'{other_description.name}_{link.get("name")}')
+            my_et.append(link)
+        # Add the joints of the other description to this description
+        for joint in other_et.findall('joint'):
+            # Change the name of the joint to avoid name conflicts
+            joint.set('name', f'{other_description.name}_{joint.get("name")}')
+            my_et.append(joint)
+        # Add the joint between the parent and child link
+        parent_link = parent_link if parent_link is not None else self.get_root()
+        child_link = child_link if child_link is not None else other.get_root()
+        joint = ET.Element('joint')
+        joint.set('name', f'{parent_link}_{child_link}_joint')
+        joint.set('type', JointDescription.pycram_type_map[joint_type])
+        if axis is not None:
+            axis_et = ET.Element('axis')
+            axis_et.set('xyz', ' '.join(map(str, [axis.x, axis.y, axis.z])))
+            joint.append(axis_et)
+        if lower_limit is not None or upper_limit is not None:
+            limit = ET.Element('limit')
+            if lower_limit is not None:
+                limit.set('lower', str(lower_limit))
+            if upper_limit is not None:
+                limit.set('upper', str(upper_limit))
+            joint.append(limit)
+        origin = ET.Element('origin')
+        origin.set('xyz', ' '.join(map(str, child_pose_wrt_parent.position_as_list())))
+        origin.set('rpy', ' '.join(map(str, euler_from_quaternion(child_pose_wrt_parent.orientation_as_list()))))
+        joint.append(origin)
+        parent = ET.Element('parent')
+        parent.set('link', parent_link)
+        joint.append(parent)
+        child = ET.Element('child')
+        child_link = f'{other_description.name}_{child_link}'
+        child.set('link', child_link)
+        joint.append(child)
+        my_et.append(joint)
+        str_description = ET.tostring(my_et, encoding='unicode')
+        self.write_description_to_file(str_description, self.original_path)
+        self.parsed_description = URDF.from_xml_string(str_description)
+        self._init_description()
+
+    def _init_description(self) -> None:
+        self._init_links()
+        self._init_joints()
 
     def load_description(self, path) -> URDF:
         with open(path, 'r') as file:
@@ -320,18 +447,26 @@ def joints(self) -> List[JointDescription]:
         :return: A list of joints descriptions of this object.
         """
         if self._joints is None:
-            self._joints = [JointDescription(joint) for joint in self.parsed_description.joints]
+            self._init_joints()
         return self._joints
 
+    def _init_joints(self) -> None:
+        self._joints = [JointDescription(joint) for joint in self.parsed_description.joints]
+        self._init_joints_map()
+
     @property
     def links(self) -> List[LinkDescription]:
         """
         :return: A list of link descriptions of this object.
         """
         if self._links is None:
-            self._links = [LinkDescription(link) for link in self.parsed_description.links]
+            self._init_links()
         return self._links
 
+    def _init_links(self) -> None:
+        self._links = [LinkDescription(link) for link in self.parsed_description.links]
+        self._init_links_map()
+
     def get_root(self) -> str:
         """
         :return: the name of the root link of this object.
@@ -429,11 +564,11 @@ def fix_missing_inertial(urdf_string: str) -> str:
         inertia_tree.getroot().append(ET.Element("mass", {"value": "0.1"}))
         inertia_tree.getroot().append(ET.Element("origin", {"rpy": "0 0 0", "xyz": "0 0 0"}))
         inertia_tree.getroot().append(ET.Element("inertia", {"ixx": "0.01",
-                                                                      "ixy": "0",
-                                                                      "ixz": "0",
-                                                                      "iyy": "0.01",
-                                                                      "iyz": "0",
-                                                                      "izz": "0.01"}))
+                                                             "ixy": "0",
+                                                             "ixz": "0",
+                                                             "iyy": "0.01",
+                                                             "iyz": "0",
+                                                             "izz": "0.01"}))
 
         # create tree from string
         tree = ET.ElementTree(ET.fromstring(urdf_string))
diff --git a/test/test_description.py b/test/test_description.py
index e377867e4..a8a110bce 100644
--- a/test/test_description.py
+++ b/test/test_description.py
@@ -36,3 +36,17 @@ def test_generate_description_from_description_file(self):
         cache_path = os.path.join(cache_path, f"{self.robot.description.name}.urdf")
         self.robot.description.generate_from_description_file(pr2_path, cache_path)
         self.assertTrue(self.world.cache_manager.is_cached(self.robot.name, self.robot.description))
+
+    def test_merge_descriptions(self):
+        milk_link_name = self.milk.description.get_root()
+        milk_joints_num = len(self.milk.description.joints)
+        cereal_joints_num = len(self.cereal.description.joints)
+        milk_link_num = len(self.milk.description.links)
+        cereal_link_num = len(self.cereal.description.links)
+        self.cereal.description.merge_description(self.milk.description)
+        self.assertEqual(len(self.cereal.description.joints), milk_joints_num + cereal_joints_num + 1)
+        self.assertEqual(len(self.cereal.description.links), milk_link_num + cereal_link_num)
+        self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
+                        in self.cereal.description.joint_map.keys())
+        self.assertTrue(milk_link_name not in self.cereal.description.link_map.keys())
+        self.assertTrue(f"{self.milk.description.name}_{milk_link_name}" in self.cereal.description.link_map.keys())

From c445b899f680d72bcb758251e797279b84b54556 Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Thu, 19 Dec 2024 19:29:50 +0100
Subject: [PATCH 04/13] [MergeDescription] added merge method for objects.

---
 src/pycram/datastructures/world.py        |  4 +-
 src/pycram/description.py                 | 10 ++-
 src/pycram/object_descriptors/generic.py  | 14 +++-
 src/pycram/object_descriptors/mjcf.py     | 13 +++-
 src/pycram/object_descriptors/urdf.py     | 79 ++++-------------------
 src/pycram/testing.py                     |  2 -
 src/pycram/world_concepts/world_object.py | 37 ++++++++---
 test/test_description.py                  | 26 ++++++--
 test/test_object.py                       | 10 ++-
 9 files changed, 99 insertions(+), 96 deletions(-)

diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py
index 13d2a9a9e..01f0bb4c0 100644
--- a/src/pycram/datastructures/world.py
+++ b/src/pycram/datastructures/world.py
@@ -412,7 +412,7 @@ def remove_object_from_simulator(self, obj: Object) -> bool:
         """
         pass
 
-    def remove_object(self, obj: Object) -> None:
+    def remove_object(self, obj: Object, remove_from_simulator: bool = True) -> None:
         """
         Remove this object from the current world.
         For the object to be removed it has to be detached from all objects it
@@ -425,7 +425,7 @@ def remove_object(self, obj: Object) -> None:
 
         obj.detach_all()
 
-        if self.remove_object_from_simulator(obj):
+        if remove_from_simulator and self.remove_object_from_simulator(obj):
             self.objects.remove(obj)
             self.remove_object_from_original_state(obj)
 
diff --git a/src/pycram/description.py b/src/pycram/description.py
index 1b647cb4f..e3f092742 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -8,7 +8,7 @@
 import trimesh
 from geometry_msgs.msg import Point
 from trimesh.parent import Geometry3D
-from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Sequence
+from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Sequence, Self
 
 from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \
     MeshVisualShape, RotatedBoundingBox
@@ -657,8 +657,8 @@ def __init__(self, path: Optional[str] = None):
         self._link_map: Optional[Dict[str, Any]] = None
         self._joint_map: Optional[Dict[str, Any]] = None
         self.original_path: Optional[str] = path
-
         if path:
+            self.xml_path = path if path.endswith((".xml", ".urdf", ".xml")) else None
             self.update_description_from_file(path)
         else:
             self._parsed_description = None
@@ -731,7 +731,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           joint_type: JointType = JointType.FIXED,
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
-                          child_pose_wrt_parent: Optional[Pose] = None) -> None:
+                          child_pose_wrt_parent: Optional[Pose] = None,
+                          in_place: bool = False) -> Union[ObjectDescription, Self]:
         """
         Merge the description of this object with the description of the other object.
 
@@ -743,6 +744,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
         :param lower_limit: The lower limit of the joint connecting the two objects.
         :param upper_limit: The upper limit of the joint connecting the two objects.
         :param child_pose_wrt_parent: The pose of the child link with respect to the parent link.
+        :param in_place: True if the merge should be done in place, False otherwise.
+        :return: The merged object description, could be a new object description if in_place is False else self.
         """
         pass
 
@@ -752,6 +755,7 @@ def update_description_from_file(self, path: str) -> None:
 
         :param path: The path of the file to update from.
         """
+        self.xml_path = path
         self._parsed_description = self.load_description(path)
 
     def update_description_from_string(self, description_string: str) -> None:
diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py
index fb2b456ec..480ea07b3 100644
--- a/src/pycram/object_descriptors/generic.py
+++ b/src/pycram/object_descriptors/generic.py
@@ -1,6 +1,6 @@
 from typing import Optional, Tuple
 
-from typing_extensions import List, Any, Union, Dict
+from typing_extensions import List, Any, Union, Dict, Self
 
 from geometry_msgs.msg import Point
 
@@ -8,7 +8,7 @@
 from ..datastructures.enums import JointType
 from ..datastructures.pose import Pose
 from ..description import JointDescription as AbstractJointDescription, LinkDescription as AbstractLinkDescription, \
-    ObjectDescription as AbstractObjectDescription
+    ObjectDescription as AbstractObjectDescription, ObjectDescription
 
 
 class NamedBoxVisualShape(BoxVisualShape):
@@ -107,8 +107,18 @@ class Joint(AbstractObjectDescription.Joint, JointDescription):
         ...
 
     def __init__(self, *args, **kwargs):
+        super().__init__()
         self._links = [LinkDescription(*args, **kwargs)]
 
+    def merge_description(self, other: ObjectDescription, parent_link: Optional[str] = None,
+                          child_link: Optional[str] = None,
+                          joint_type: JointType = JointType.FIXED,
+                          axis: Optional[Point] = None,
+                          lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                          child_pose_wrt_parent: Optional[Pose] = None,
+                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+        raise NotImplementedError
+
     def load_description(self, path: str) -> Any:
         ...
 
diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py
index d9475d0b2..42023853c 100644
--- a/src/pycram/object_descriptors/mjcf.py
+++ b/src/pycram/object_descriptors/mjcf.py
@@ -6,14 +6,14 @@
 import numpy as np
 from dm_control import mjcf
 from geometry_msgs.msg import Point
-from typing_extensions import Union, List, Optional, Dict, Tuple
+from typing_extensions import Union, List, Optional, Dict, Tuple, Self
 
 from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \
     SphereVisualShape, MeshVisualShape, VisualShapeUnion
 from ..datastructures.enums import JointType, MJCFGeomType, MJCFJointType, Shape
 from ..datastructures.pose import Pose
 from ..description import JointDescription as AbstractJointDescription, \
-    LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription
+    LinkDescription as AbstractLinkDescription, ObjectDescription as AbstractObjectDescription, ObjectDescription
 from ..failures import MultiplePossibleTipLinks
 from ..ros.ros_tools import get_parameter
 
@@ -357,6 +357,15 @@ def __init__(self):
         self.virtual_joint_names = []
         self._meshes_dir: Optional[str] = None
 
+    def merge_description(self, other: ObjectDescription, parent_link: Optional[str] = None,
+                          child_link: Optional[str] = None,
+                          joint_type: JointType = JointType.FIXED,
+                          axis: Optional[Point] = None,
+                          lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
+                          child_pose_wrt_parent: Optional[Pose] = None,
+                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+        raise NotImplementedError
+
     @property
     def mesh_dir(self) -> Optional[str]:
         try:
diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py
index f4c97d503..8023a1e33 100644
--- a/src/pycram/object_descriptors/urdf.py
+++ b/src/pycram/object_descriptors/urdf.py
@@ -8,7 +8,7 @@
 import numpy as np
 from geometry_msgs.msg import Point
 from tf.transformations import quaternion_from_euler, euler_from_quaternion
-from typing_extensions import Union, List, Optional, Dict, Tuple, Type
+from typing_extensions import Union, List, Optional, Dict, Tuple, Type, Self
 from urdf_parser_py import urdf
 from urdf_parser_py.urdf import (URDF, Collision, Box as URDF_Box, Cylinder as URDF_Cylinder,
                                  Sphere as URDF_Sphere, Mesh as URDF_Mesh)
@@ -287,7 +287,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           joint_type: JointType = JointType.FIXED,
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
-                          child_pose_wrt_parent: Optional[Pose] = None) -> None:
+                          child_pose_wrt_parent: Optional[Pose] = None,
+                          in_place: bool = False) -> Union[ObjectDescription, Self]:
         other_description = other.parsed_description
         if child_pose_wrt_parent is None:
             child_pose_wrt_parent = Pose()
@@ -296,85 +297,29 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
         child_link = f"{other_description.name}_" + original_child_link
         parent_link = parent_link if parent_link is not None else self.get_root()
 
+        description = self if in_place else copy.deepcopy(self)
+
         # Add the links of the other description to this description
         for link in other_description.links:
             # Change the name of the link to avoid name conflicts
             link = copy.deepcopy(link)
             link.name = f'{other_description.name}_{link.name}'
-            self.add_link_from_parsed_description(link)
+            description.add_link_from_parsed_description(link)
 
         # Add the joints of the other description to this description
         for joint in other_description.joints:
             # Change the name of the joint to avoid name conflicts
             joint = copy.deepcopy(joint)
             joint.name = f'{other_description.name}_{joint.name}'
-            self.add_joint_from_parsed_description(joint)
+            description.add_joint_from_parsed_description(joint)
 
         # Add the joint between the parent and child link
-        self.add_joint(f'{parent_link}_{original_child_link}_joint', child_link, joint_type,
-                       axis=axis, origin=child_pose_wrt_parent, parent=parent_link,
-                       lower_limit=lower_limit, upper_limit=upper_limit)
+        description.add_joint(f'{parent_link}_{original_child_link}_joint', child_link, joint_type,
+                              axis=axis, origin=child_pose_wrt_parent, parent=parent_link,
+                              lower_limit=lower_limit, upper_limit=upper_limit)
 
-        self.write_description_to_file(self.parsed_description.to_xml_string(), self.original_path)
-
-    def merge_description_using_et(self, other: ObjectDescription, parent_link: Optional[str] = None,
-                                   child_link: Optional[str] = None,
-                                   joint_type: JointType = JointType.FIXED,
-                                   axis: Optional[Point] = None,
-                                   lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
-                                   child_pose_wrt_parent: Optional[Pose] = None) -> None:
-        """
-        Merge the description of another object into this object description using the element tree library.
-        parameters are the same as in :meth:`pycram.description.ObjectDescription.merge_description`.
-        """
-        other_description = other.parsed_description
-        if child_pose_wrt_parent is None:
-            child_pose_wrt_parent = Pose()
-        other_et = ET.fromstring(other_description.to_xml_string())
-        my_et = ET.fromstring(self.parsed_description.to_xml_string())
-        # Add the links of the other description to this description
-        for link in other_et.findall('link'):
-            # Change the name of the link to avoid name conflicts
-            link.set('name', f'{other_description.name}_{link.get("name")}')
-            my_et.append(link)
-        # Add the joints of the other description to this description
-        for joint in other_et.findall('joint'):
-            # Change the name of the joint to avoid name conflicts
-            joint.set('name', f'{other_description.name}_{joint.get("name")}')
-            my_et.append(joint)
-        # Add the joint between the parent and child link
-        parent_link = parent_link if parent_link is not None else self.get_root()
-        child_link = child_link if child_link is not None else other.get_root()
-        joint = ET.Element('joint')
-        joint.set('name', f'{parent_link}_{child_link}_joint')
-        joint.set('type', JointDescription.pycram_type_map[joint_type])
-        if axis is not None:
-            axis_et = ET.Element('axis')
-            axis_et.set('xyz', ' '.join(map(str, [axis.x, axis.y, axis.z])))
-            joint.append(axis_et)
-        if lower_limit is not None or upper_limit is not None:
-            limit = ET.Element('limit')
-            if lower_limit is not None:
-                limit.set('lower', str(lower_limit))
-            if upper_limit is not None:
-                limit.set('upper', str(upper_limit))
-            joint.append(limit)
-        origin = ET.Element('origin')
-        origin.set('xyz', ' '.join(map(str, child_pose_wrt_parent.position_as_list())))
-        origin.set('rpy', ' '.join(map(str, euler_from_quaternion(child_pose_wrt_parent.orientation_as_list()))))
-        joint.append(origin)
-        parent = ET.Element('parent')
-        parent.set('link', parent_link)
-        joint.append(parent)
-        child = ET.Element('child')
-        child_link = f'{other_description.name}_{child_link}'
-        child.set('link', child_link)
-        joint.append(child)
-        my_et.append(joint)
-        str_description = ET.tostring(my_et, encoding='unicode')
-        self.write_description_to_file(str_description, self.original_path)
-        self.parsed_description = URDF.from_xml_string(str_description)
-        self._init_description()
+        description.write_description_to_file(description.parsed_description.to_xml_string(), description.xml_path)
+        return description
 
     def _init_description(self) -> None:
         self._init_links()
diff --git a/src/pycram/testing.py b/src/pycram/testing.py
index c5e66d609..ff0003901 100644
--- a/src/pycram/testing.py
+++ b/src/pycram/testing.py
@@ -70,7 +70,5 @@ def setUpClass(cls):
         cls.cereal = Object("cereal", Cereal, "breakfast_cereal.stl",
                             pose=Pose([1.3, 0.7, 0.95]))
 
-
-
 class BulletWorldGUITestCase(BulletWorldTestCase):
     render_mode = WorldMode.GUI
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 11a36b26f..2c0d5c8cc 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -10,7 +10,7 @@
 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 typing_extensions import Type, Optional, Dict, Tuple, List, Union, Self
 
 import pycrap
 from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState,
@@ -387,6 +387,7 @@ def _spawn_object_and_get_id(self) -> int:
             logging.error(
                 "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or"
                 " the name of an URDF string on the parameter server.")
+            logging.error(e)
             os.remove(path)
             raise e
 
@@ -643,20 +644,18 @@ def base_origin_shift(self) -> np.ndarray:
         return np.array(self.get_position_as_list()) - np.array(self.get_base_position_as_list())
 
     def __repr__(self):
-        skip_attr = ["links", "joints", "description", "attachments"]
-        return self.__class__.__qualname__ + f"(name={self.name}, object_type={self.obj_type.name}, file_path={self.path}, pose={self.pose}, world={self.world})"
+        return self.__class__.__qualname__ + (f"(name={self.name}, object_type={self.obj_type.name},"
+                                              f" file_path={self.path}, pose={self.pose}, world={self.world})")
 
-
-    def remove(self) -> None:
+    def remove(self, remove_from_simulator: bool = True) -> None:
         """
         Remove this object from the World it currently resides in.
         For the object to be removed it has to be detached from all objects it
-        is currently attached to. After this call world remove object
-        to remove this Object from the simulation/world.
-        """
-        # owlready2.destroy_entity(self.ontology_individual)
-        self.world.remove_object(self)
+        is currently attached to. Then remove this Object from the simulation/world if remove_from_simulator is True.
 
+        :param remove_from_simulator: If True the object will be removed from the simulator.
+        """
+        self.world.remove_object(self, remove_from_simulator=remove_from_simulator)
 
     def reset(self, remove_saved_states=False) -> None:
         """
@@ -691,6 +690,24 @@ def is_a_robot(self) -> bool:
         """
         return issubclass(self.obj_type, pycrap.Robot)
 
+    def merge(self, other: Object, name: Optional[str] = None, pose: Optional[Pose] = None) -> Object:
+        """
+        Merge the object with another object. This is done by merging the descriptions of the objects,
+        removing the other object and updating the links and joints of this object.
+
+        :param other: The object to merge with.
+        :param name: The name of the merged object.
+        :param pose: The pose of the merged object.
+        :return: The merged object.
+        """
+        pose = self.pose if pose is None else pose
+        child_pose = self.local_transformer.transform_pose(other.pose, self.tf_frame)
+        description = self.description.merge_description(other.description, child_pose_wrt_parent=child_pose)
+        name = self.name if name is None else name
+        other.remove()
+        self.remove()
+        return Object(name, self.obj_type, self.path, description=description, pose=pose, world=self.world)
+
     def attach(self,
                child_object: Object,
                parent_link: Optional[str] = None,
diff --git a/test/test_description.py b/test/test_description.py
index a8a110bce..1de25c42f 100644
--- a/test/test_description.py
+++ b/test/test_description.py
@@ -43,10 +43,22 @@ def test_merge_descriptions(self):
         cereal_joints_num = len(self.cereal.description.joints)
         milk_link_num = len(self.milk.description.links)
         cereal_link_num = len(self.cereal.description.links)
-        self.cereal.description.merge_description(self.milk.description)
-        self.assertEqual(len(self.cereal.description.joints), milk_joints_num + cereal_joints_num + 1)
-        self.assertEqual(len(self.cereal.description.links), milk_link_num + cereal_link_num)
-        self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
-                        in self.cereal.description.joint_map.keys())
-        self.assertTrue(milk_link_name not in self.cereal.description.link_map.keys())
-        self.assertTrue(f"{self.milk.description.name}_{milk_link_name}" in self.cereal.description.link_map.keys())
+        for in_place in [False, True]:
+            description = self.cereal.description.merge_description(self.milk.description, in_place=in_place)
+            if in_place:
+                self.assertTrue(description is self.cereal.description)
+                self.assertEqual(len(self.cereal.description.joints), milk_joints_num + cereal_joints_num + 1)
+                self.assertEqual(len(self.cereal.description.links), milk_link_num + cereal_link_num)
+                self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
+                                in self.cereal.description.joint_map.keys())
+                self.assertTrue(milk_link_name not in self.cereal.description.link_map.keys())
+                self.assertTrue(f"{self.milk.description.name}_{milk_link_name}" in self.cereal.description.link_map.keys())
+            else:
+                self.assertFalse(self.cereal.description is description)
+                self.assertEqual(len(description.joints), milk_joints_num + cereal_joints_num + 1)
+                self.assertEqual(len(description.links), milk_link_num + cereal_link_num)
+                self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
+                                in description.joint_map.keys())
+                self.assertTrue(milk_link_name not in description.link_map.keys())
+                self.assertTrue(
+                    f"{self.milk.description.name}_{milk_link_name}" in description.link_map.keys())
diff --git a/test/test_object.py b/test/test_object.py
index 39b4830fa..aa4a22248 100644
--- a/test/test_object.py
+++ b/test/test_object.py
@@ -166,6 +166,14 @@ def test_object_equal(self):
         self.assertNotEqual(self.milk, self.cereal)
         self.assertNotEqual(self.milk, self.world)
 
+    def test_merge(self):
+        cereal = Object("cereal2", Food, "breakfast_cereal.stl")
+        milk = Object("milk2", Milk, "milk.stl")
+        cereal_milk = cereal.merge(milk)
+        self.assertEqual(len(cereal_milk.links), len(cereal.links) + len(milk.links))
+        self.assertEqual(len(cereal_milk.joints), len(cereal.joints) + len(milk.joints) + 1)
+        cereal_milk.remove()
+
 
 class GenericObjectTestCase(BulletWorldTestCase):
 
@@ -183,7 +191,7 @@ def test_querying(self):
         r = list(filter(lambda x:  x in Milk.instances(), self.world.ontology.individuals()))
         self.assertEqual(len(r), 1)
 
-        milk2  = Object("milk2", Milk, "milk.stl")
+        milk2 = Object("milk2", Milk, "milk.stl")
 
         r = list(filter(lambda x:  x in Milk.instances(), self.world.ontology.individuals()))
 

From fe0bf361c7c46859e77d7efe9bb7f47e1e8f3f96 Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Thu, 19 Dec 2024 19:35:01 +0100
Subject: [PATCH 05/13] [MergeBoundingBox] Did some cleaning.

---
 src/pycram/datastructures/dataclasses.py | 108 +----------------------
 1 file changed, 1 insertion(+), 107 deletions(-)

diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index 1464a35dc..222a465ce 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -1,6 +1,5 @@
 from __future__ import annotations
 
-import os
 from abc import ABC, abstractmethod
 from copy import deepcopy, copy
 from dataclasses import dataclass, fields, field
@@ -240,7 +239,7 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
             z = Continuous("z")
 
             all_intervals = [(box.get_min(), box.get_max()) for box in bounding_boxes]
-            event = SimpleEvent()
+            interval = None
             for min_point, max_point in all_intervals:
                 event = SimpleEvent({x: closed(min_point[0], max_point[0]), y: closed(min_point[1], max_point[1]),
                                      z: closed(min_point[2], max_point[2])}).as_composite_set()
@@ -248,7 +247,6 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
                     interval = event
                 else:
                     interval = interval.union_with(event) - interval.intersection_with(event)
-            vertices, faces = AxisAlignedBoundingBox.get_mesh_data_from_interval(interval)
             if plot:
                 fig = go.Figure(interval.plot(), interval.plotly_layout())
                 fig.update_layout(title="Merged Bounding Boxes")
@@ -265,110 +263,6 @@ def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoun
                 shell.save(save_mesh_to)
             return shell
 
-    @staticmethod
-    def get_mesh_data_from_interval(interval: Interval) -> Tuple[np.ndarray, np.ndarray]:
-        # form cartesian product of all intervals
-        intervals = [value.simple_sets for _, value in sorted(self.items())]
-        simple_events = list(itertools.product(*intervals))
-        traces = []
-
-        # shortcut for the dimensions
-        x, y, z = 0, 1, 2
-
-        # for every atomic interval
-        for simple_event in simple_events:
-            # Create a 3D mesh trace for the rectangle
-            traces.append(go.Mesh3d(
-                # 8 vertices of a cube
-                x=[simple_event[x].lower, simple_event[x].lower, simple_event[x].upper, simple_event[x].upper,
-                   simple_event[x].lower, simple_event[x].lower, simple_event[x].upper, simple_event[x].upper],
-                y=[simple_event[y].lower, simple_event[y].upper, simple_event[y].upper, simple_event[y].lower,
-                   simple_event[y].lower, simple_event[y].upper, simple_event[y].upper, simple_event[y].lower],
-                z=[simple_event[z].lower, simple_event[z].lower, simple_event[z].lower, simple_event[z].lower,
-                   simple_event[z].upper, simple_event[z].upper, simple_event[z].upper, simple_event[z].upper],
-                # i, j and k give the vertices of triangles
-                i=[7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2],
-                j=[3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3],
-                k=[0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6],
-                flatshading=True
-            ))
-
-    def merge_with_other_box(self, other_box: 'AxisAlignedBoundingBox',
-                             get_mesh: bool = False,
-                             save_mesh_to: Optional[str] = None) -> Union[List[List[float]], trimesh.parent.Geometry3D]:
-        """
-        Merge the axis-aligned bounding box with another axis-aligned bounding box.
-
-        :param other_box: The other axis-aligned bounding box.
-        :param get_mesh: If True, return the mesh of the merged bounding boxes.
-        :param save_mesh_to: The file path to save the mesh to.
-        :return: The points of the final shape as a list of lists of floats or the mesh of the merged bounding boxes.
-        """
-        intersection_points, inside_points = self.get_intersection_points_with_other_box(other_box,
-                                                                                         return_inside_points=True,
-                                                                                         as_list=True)
-        all_points = [point for box in [self, other_box] for point in box.get_points_list()
-                      if point not in inside_points]
-        all_points.extend(intersection_points)
-
-        if get_mesh:
-            # create a mesh from the points
-            mesh = trimesh.Trimesh(vertices=np.array(all_points), faces=trimesh.convex.convex_hull(all_points))
-            if save_mesh_to is not None:
-                mesh.export(save_mesh_to)
-            return mesh
-        else:
-            return all_points
-
-    def get_intersection_points_with_other_box(self, other_box: 'AxisAlignedBoundingBox',
-                                               return_inside_points: bool = False,
-                                               as_list: bool = False) -> Union[Tuple[List, List], List]:
-        """
-        Get the intersection points with another axis-aligned bounding box.
-
-        :param other_box: The other axis-aligned bounding box.
-        :param return_inside_points: If True, return the points that are inside the bounding boxes.
-        :param as_list: If True, return the points as a list of lists of floats.
-        :return: The intersection points as a list of points.
-        """
-        intersection_box = self.get_intersection_box_with_other_box(other_box)
-        intersection_points = []
-        inside_points = []
-        for point in intersection_box.get_points():
-            if not self.is_point_inside(point) and not other_box.is_point_inside(point):
-                intersection_points.append(get_point_as_list(point) if as_list else point)
-            elif return_inside_points:
-                inside_points.append(get_point_as_list(point) if as_list else point)
-        if return_inside_points:
-            return intersection_points, inside_points
-        return intersection_points
-
-    def get_intersection_box_with_other_box(self, other_box: 'AxisAlignedBoundingBox') -> 'AxisAlignedBoundingBox':
-        """
-        Get the intersection box with another axis-aligned bounding box.
-
-        :param other_box: The other axis-aligned bounding box.
-        :return: The intersection box.
-        """
-        min_x = max(self.min_x, other_box.min_x)
-        min_y = max(self.min_y, other_box.min_y)
-        min_z = max(self.min_z, other_box.min_z)
-        max_x = min(self.max_x, other_box.max_x)
-        max_y = min(self.max_y, other_box.max_y)
-        max_z = min(self.max_z, other_box.max_z)
-        return AxisAlignedBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z)
-
-    def is_point_inside(self, point: Point) -> bool:
-        """
-        Check if a point is inside the axis-aligned bounding box.
-
-        :param point: The point to check
-        :return: True if the point is inside the bounding box, False otherwise
-        """
-        return (self.min_x < point.x < self.max_x
-                and self.min_y < point.y < self.max_y
-                and self.min_z < point.z < self.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.

From 40c7b206924f06f59a3b955ca72035ad0af95b7b Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Tue, 14 Jan 2025 11:29:59 +0100
Subject: [PATCH 06/13] [MergeBoundingBox] added two options for merging boxes
 into mesh.

use shifting instead of transforming an axis_aligned_bounding_box
---
 src/pycram/datastructures/dataclasses.py  | 252 +++++++++++++---------
 src/pycram/description.py                 |  11 +-
 src/pycram/world_concepts/world_object.py |   6 +-
 test/test_links.py                        |  25 +--
 test/test_object.py                       |  21 +-
 5 files changed, 184 insertions(+), 131 deletions(-)

diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index 222a465ce..a6a917f8a 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -1,16 +1,16 @@
 from __future__ import annotations
 
+import itertools
 from abc import ABC, abstractmethod
 from copy import deepcopy, copy
 from dataclasses import dataclass, fields, field
 
 import numpy as np
 import trimesh
+from matplotlib import pyplot as plt
 from random_events.variable import Continuous
-from random_events.interval import closed, Interval
-from random_events.product_algebra import SimpleEvent
-from pyvista import PolyData
-import pyvista as pv
+from random_events.interval import closed
+from random_events.product_algebra import SimpleEvent, Event
 import plotly.graph_objects as go
 from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Sequence
 
@@ -108,6 +108,109 @@ class BoundingBox:
     max_y: float
     max_z: float
 
+    @staticmethod
+    def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[BoundingBox],
+                                                save_mesh_to: Optional[str] = None,
+                                                use_random_events: bool = True,
+                                                plot: bool = False) -> trimesh.parent.Geometry3D:
+        """
+        Merge multiple axis-aligned bounding boxes into a single mesh.
+
+        :param bounding_boxes: The list of axis-aligned bounding boxes.
+        :param save_mesh_to: The file path to save the mesh to.
+        :param use_random_events: If True, use random events to compute the new shape, otherwise use pyvista.
+        :param plot: If True, plot the mesh.
+        :return: The mesh of the merged bounding boxes.
+        """
+        if use_random_events:
+            x = Continuous("x")
+            y = Continuous("y")
+            z = Continuous("z")
+
+            all_intervals = [(box.get_min(), box.get_max()) for box in bounding_boxes]
+            event = Event()
+            for min_point, max_point in all_intervals:
+                new_event = SimpleEvent({x: closed(min_point[0], max_point[0]), y: closed(min_point[1], max_point[1]),
+                                         z: closed(min_point[2], max_point[2])}).as_composite_set()
+                event = event.union_with(new_event)
+            if plot:
+                fig = go.Figure(event.plot(), event.plotly_layout())
+                fig.update_layout(title="Merged Bounding Boxes")
+                fig.show()
+            mesh = BoundingBox.get_mesh_from_event(event)
+        else:
+            mesh = BoundingBox.get_mesh_from_boxes(bounding_boxes)
+        if plot:
+            mesh.show()
+            BoundingBox.plot_3d_points([mesh.vertices])
+        if save_mesh_to is not None:
+            mesh.export(save_mesh_to)
+        return mesh
+
+    @staticmethod
+    def get_mesh_from_boxes(boxes: List[BoundingBox]) -> trimesh.parent.Geometry3D:
+        """
+        Get the mesh from the boxes
+
+        :param boxes: The list of boxes
+        :return: The mesh.
+        """
+        # for every atomic interval
+        all_vertices = []
+        all_faces = []
+        for i, box in enumerate(boxes):
+            # Create a 3D mesh trace for the rectangle
+            all_vertices.extend(box.get_points_list())
+            all_faces.extend((np.array(BoundingBox.get_box_faces()) + i * 8).tolist())
+        return trimesh.Trimesh(np.array(all_vertices), np.array(all_faces))
+
+    @staticmethod
+    def get_mesh_from_event(event: Event) -> trimesh.parent.Geometry3D:
+        """
+        Get the mesh from the event.
+
+        :param event: The event.
+        :return: The mesh.
+        """
+        # form cartesian product of all intervals
+        intervals = [value.simple_sets for simple_event in event.simple_sets for _, value in simple_event.items()]
+        simple_events = list(itertools.product(*intervals))
+
+        # for every atomic interval
+        all_vertices = []
+        all_faces = []
+        for i, simple_event in enumerate(simple_events):
+            x, y, z = 0, 1, 2
+            for j in range(2):
+                x, y, z = x + j*3, y + j*3, z + j*3
+                # Create a 3D mesh trace for the rectangle
+                all_vertices.extend([[simple_event[x].lower, simple_event[y].lower, simple_event[z].lower],
+                                [simple_event[x].lower, simple_event[y].lower, simple_event[z].upper],
+                                [simple_event[x].lower, simple_event[y].upper, simple_event[z].lower],
+                                [simple_event[x].lower, simple_event[y].upper, simple_event[z].upper],
+                                [simple_event[x].upper, simple_event[y].lower, simple_event[z].lower],
+                                [simple_event[x].upper, simple_event[y].lower, simple_event[z].upper],
+                                [simple_event[x].upper, simple_event[y].upper, simple_event[z].lower],
+                                [simple_event[x].upper, simple_event[y].upper, simple_event[z].upper]])
+                all_faces.extend((np.array(BoundingBox.get_box_faces()) + i*16 + j*8).tolist())
+        return trimesh.Trimesh(np.array(all_vertices), np.array(all_faces))
+
+    @staticmethod
+    def get_box_faces() -> List[List[int]]:
+        return [[0, 1, 2], [2, 3, 1], [4, 5, 6], [6, 7, 5],
+                [0, 1, 4], [4, 5, 1], [2, 3, 6], [6, 7, 2],
+                [0, 2, 4], [4, 6, 2], [1, 3, 5], [5, 7, 3]]
+
+    @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])
+
     @property
     def origin(self) -> List[float]:
         return [(self.min_x + self.max_x) / 2, (self.min_y + self.max_y) / 2, (self.min_z + self.max_z) / 2]
@@ -188,10 +291,36 @@ def height(self) -> float:
     def depth(self) -> float:
         return self.max_y - self.min_y
 
+    @staticmethod
+    def plot_3d_points(list_of_points: List[np.ndarray]):
+        fig = plt.figure()
+        ax = fig.add_subplot(projection='3d')
+
+        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)
+
+        plt.show()
+
 
 @dataclass
 class AxisAlignedBoundingBox(BoundingBox):
 
+    def get_rotated_box(self, transform: Transform) -> RotatedBoundingBox:
+        """
+        Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box.
+
+        :return: The transformed axis-aligned bounding box
+        """
+        return RotatedBoundingBox.from_min_max(self.get_min(), self.get_max(), transform)
+
     @classmethod
     def from_origin_and_half_extents(cls, origin: Point, half_extents: Point):
         """
@@ -205,7 +334,7 @@ def from_origin_and_half_extents(cls, origin: Point, half_extents: Point):
         return cls.from_min_max(min_point, max_point)
 
     @classmethod
-    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox':
+    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> AxisAlignedBoundingBox:
         """
         Set the axis-aligned bounding box from multiple axis-aligned bounding boxes.
 
@@ -219,71 +348,15 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo
         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)
 
-    @staticmethod
-    def merge_multiple_bounding_boxes_into_mesh(bounding_boxes: List[AxisAlignedBoundingBox],
-                                                save_mesh_to: Optional[str] = None,
-                                                use_random_events: bool = True,
-                                                plot: bool = True) -> Union[PolyData, Interval]:
-        """
-        Merge multiple axis-aligned bounding boxes into a single mesh.
-
-        :param bounding_boxes: The list of axis-aligned bounding boxes.
-        :param save_mesh_to: The file path to save the mesh to.
-        :param use_random_events: If True, use random events to compute the new shape, otherwise use pyvista.
-        :param plot: If True, plot the mesh.
-        :return: The mesh of the merged bounding boxes.
-        """
-        if use_random_events:
-            x = Continuous("x")
-            y = Continuous("y")
-            z = Continuous("z")
-
-            all_intervals = [(box.get_min(), box.get_max()) for box in bounding_boxes]
-            interval = None
-            for min_point, max_point in all_intervals:
-                event = SimpleEvent({x: closed(min_point[0], max_point[0]), y: closed(min_point[1], max_point[1]),
-                                     z: closed(min_point[2], max_point[2])}).as_composite_set()
-                if interval is None:
-                    interval = event
-                else:
-                    interval = interval.union_with(event) - interval.intersection_with(event)
-            if plot:
-                fig = go.Figure(interval.plot(), interval.plotly_layout())
-                fig.update_layout(title="Merged Bounding Boxes")
-                fig.show()
-            return interval
-        else:
-            all_points = [point for box in bounding_boxes for point in box.get_points_list()]
-            cloud = pv.PolyData(all_points)
-            volume = cloud.delaunay_3d(alpha=2.)
-            shell = volume.extract_geometry()
-            if plot:
-                shell.plot()
-            if save_mesh_to is not None:
-                shell.save(save_mesh_to)
-            return shell
-
-    def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox':
+    def shift_by(self, shift: Point) -> AxisAlignedBoundingBox:
         """
-        Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box.
+        Shift the axis-aligned bounding box by a given shift.
 
-        :param transform: The transformation to apply
-        :return: The transformed axis-aligned bounding box
+        :param shift: The shift to apply
+        :return: The shifted 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])
+        return AxisAlignedBoundingBox(self.min_x + shift.x, self.min_y + shift.y, self.min_z + shift.z,
+                                      self.max_x + shift.x, self.max_y + shift.y, self.max_z + shift.z)
 
 
 @dataclass
@@ -291,16 +364,20 @@ 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
+                 transform: Optional[Transform] = None, points: Optional[List[Point]] = None):
+        """
+        Set the rotated bounding box from a minimum and maximum point.
+        :param transform: The transformation
+        :param points: The points of the rotated bounding box.
+        """
+        self.transform: Optional[Transform] = transform
+        super().__init__(min_x, min_y, min_z, max_x, max_y, max_z)
         self._points: Optional[List[Point]] = points
 
     @classmethod
-    def from_min_max(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: Optional[Transform] = None):
         """
         Set the rotated bounding box from a minimum, maximum point, and a transformation.
 
@@ -310,35 +387,12 @@ def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], tr
         """
         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(self, transform: Optional[Transform] = None) -> List[Point]:
+    def get_points(self) -> 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]])
+        points_array = np.array([[point.x, point.y, point.z] for point in super().get_points()])
+        if self._points is None:
             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
diff --git a/src/pycram/description.py b/src/pycram/description.py
index ff14c1f91..1e63f3c56 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -214,9 +214,9 @@ def name(self) -> str:
         """
         return self.description.name
 
-    def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self, shift_to_link_position: bool = True) -> AxisAlignedBoundingBox:
         """
-        :param transform_to_link_pose: If True, return the bounding box transformed to the link pose.
+        :param shift_to_link_position: 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.
         """
@@ -224,8 +224,8 @@ def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) ->
             return self.world.get_link_axis_aligned_bounding_box(self)
         except NotImplementedError:
             bounding_box = self.get_axis_aligned_bounding_box_from_geometry()
-            if transform_to_link_pose:
-                return bounding_box.get_transformed_box(self.transform)
+            if shift_to_link_position:
+                return bounding_box.shift_by(self.pose.position)
             else:
                 return bounding_box
 
@@ -237,8 +237,7 @@ def get_rotated_bounding_box(self) -> RotatedBoundingBox:
         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)
+            return self.get_axis_aligned_bounding_box_from_geometry().get_rotated_box(self.transform)
 
     def get_axis_aligned_bounding_box_from_geometry(self) -> AxisAlignedBoundingBox:
         if isinstance(self.geometry, List):
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 11a36b26f..7fb53b373 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -1402,15 +1402,15 @@ def links_colors(self) -> Dict[str, Color]:
         """
         return self.world.get_colors_of_object_links(self)
 
-    def get_axis_aligned_bounding_box(self, transform_to_object_pose: bool = True) -> AxisAlignedBoundingBox:
+    def get_axis_aligned_bounding_box(self, shift_to_object_position: bool = True) -> AxisAlignedBoundingBox:
         """
         Return the axis aligned bounding box of this object.
 
-        :param transform_to_object_pose: If True, the bounding box will be transformed to fit object pose.
+        :param shift_to_object_position: If True, the bounding box will be shifted to the object position.
         :return: The axis aligned bounding box of this object.
         """
         if self.has_one_link:
-            return self.root_link.get_axis_aligned_bounding_box(transform_to_object_pose)
+            return self.root_link.get_axis_aligned_bounding_box(shift_to_object_position)
         else:
             return self.world.get_object_axis_aligned_bounding_box(self)
 
diff --git a/test/test_links.py b/test/test_links.py
index 3cd354a35..d5777664a 100644
--- a/test/test_links.py
+++ b/test/test_links.py
@@ -1,10 +1,9 @@
 import numpy as np
-from matplotlib import pyplot as plt
 from tf.transformations import quaternion_from_euler
 from typing_extensions import List
 
 from pycram.testing import BulletWorldTestCase
-from pycram.datastructures.dataclasses import Color
+from pycram.datastructures.dataclasses import Color, BoundingBox as BB
 from pycram.datastructures.pose import Pose
 
 
@@ -18,7 +17,7 @@ def test_get_convex_hull(self):
         self.assertTrue(len(hull.faces) > 0)
         plot = False
         if plot:
-            self.plot_3d_points([hull.vertices])
+            BB.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()))
@@ -28,25 +27,7 @@ def test_rotated_bounding_box(self):
         rot_points = np.array(rbb.get_points_list())
         plot = False
         if plot:
-            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')
-
-        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)
-
-        plt.show()
+            BB.plot_3d_points([aabb_points, rot_points])
 
     def test_add_constraint(self):
         milk_link = self.milk.root_link
diff --git a/test/test_object.py b/test/test_object.py
index 39b4830fa..4394bc3e1 100644
--- a/test/test_object.py
+++ b/test/test_object.py
@@ -1,10 +1,12 @@
 import numpy as np
+import trimesh.parent
+from tf.transformations import quaternion_from_euler
 
 from pycram.testing import BulletWorldTestCase
 
 from pycram.datastructures.enums import JointType, ObjectType
 from pycram.datastructures.pose import Pose
-from pycram.datastructures.dataclasses import Color
+from pycram.datastructures.dataclasses import Color, BoundingBox as BB
 from pycram.failures import UnsupportedFileExtension
 from pycram.world_concepts.world_object import Object
 from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription
@@ -166,6 +168,23 @@ def test_object_equal(self):
         self.assertNotEqual(self.milk, self.cereal)
         self.assertNotEqual(self.milk, self.world)
 
+    def test_merge_bounding_box(self):
+        cereal_2 = Object("cereal2", Food, "breakfast_cereal.stl",
+                          pose=self.cereal.pose)
+        cereal_2.set_orientation(quaternion_from_euler(0, 0, np.pi / 2).tolist())
+        cereal_bbox = self.cereal.get_axis_aligned_bounding_box(False)
+        cereal_2_bbox = cereal_2.get_axis_aligned_bounding_box(False)
+        plot = False
+        if plot:
+            BB.plot_3d_points([np.array(cereal_bbox.get_points_list()), np.array(cereal_2_bbox.get_points_list())])
+        for use_random_events in [True, False]:
+            merged_bbox_mesh = BB.merge_multiple_bounding_boxes_into_mesh([cereal_bbox, cereal_2_bbox],
+                                                                          use_random_events=use_random_events,
+                                                                          plot=plot)
+            self.assertTrue(isinstance(merged_bbox_mesh, trimesh.parent.Geometry3D))
+            self.assertEqual(merged_bbox_mesh.vertices.shape[0], 24 if use_random_events else 16)
+            self.assertEqual(merged_bbox_mesh.faces.shape[0], 48 if use_random_events else 24)
+
 
 class GenericObjectTestCase(BulletWorldTestCase):
 

From 4b5ba35d309e499d12e0b15b29d21a1aa8d18f75 Mon Sep 17 00:00:00 2001
From: A_Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Thu, 16 Jan 2025 22:57:14 +0100
Subject: [PATCH 07/13] [PhysicalBody] moved ontology concept to physical body
 class.

---
 src/pycram/datastructures/world_entity.py |  8 +++++++-
 src/pycram/world_concepts/world_object.py | 11 +++--------
 2 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py
index fe9756bf2..da9ba55c1 100644
--- a/src/pycram/datastructures/world_entity.py
+++ b/src/pycram/datastructures/world_entity.py
@@ -166,9 +166,15 @@ class PhysicalBody(WorldEntity, HasConcept, ABC):
     The ontology concept of this entity.
     """
 
-    def __init__(self, body_id: int, world: World):
+    def __init__(self, body_id: int, world: World, concept: Type[PhysicalObject] = PhysicalObject):
         WorldEntity.__init__(self, body_id, world)
         HasConcept.__init__(self)
+
+        # set ontology related information
+        self.ontology_concept = concept
+        if not self.world.is_prospection_world:
+            self.ontology_individual = self.ontology_concept(namespace=self.world.ontology.ontology)
+
         self.local_transformer = LocalTransformer()
         self._is_translating: Optional[bool] = None
         self._is_rotating: Optional[bool] = None
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index ec07614b6..5fb92d180 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -36,7 +36,7 @@
 from ..robot_description import RobotDescriptionManager, RobotDescription
 from ..world_concepts.constraints import Attachment
 from ..datastructures.mixins import HasConcept
-from pycrap import PhysicalObject, ontology, Base, Agent
+from pycrap import PhysicalObject, ontology, Base, Agent, Robot
 
 Link = ObjectDescription.Link
 
@@ -83,15 +83,10 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str]
         :param scale_mesh: The scale of the mesh.
         """
 
-        super().__init__(-1, world if world is not None else World.current_world)
+        super().__init__(-1, world if world is not None else World.current_world, concept)
 
         pose = Pose() if pose is None else pose
 
-        # set ontology related information
-        self.ontology_concept = concept
-        if not self.world.is_prospection_world:
-            self.ontology_individual = self.ontology_concept(namespace=self.world.ontology.ontology)
-
         self.name: str = name
         self.path: Optional[str] = path
 
@@ -111,7 +106,7 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str]
             self.description.update_description_from_file(self.path)
 
         # if the object is an agent in the belief state
-        if Agent in self.ontology_concept.is_a and not self.world.is_prospection_world:
+        if Robot in self.ontology_concept.is_a and not self.world.is_prospection_world:
             self._update_world_robot_and_description()
 
         self.id = self._spawn_object_and_get_id()

From c1cc4ab73e657791822a3dfb64a67f05e83ae3f8 Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Fri, 17 Jan 2025 11:11:12 +0100
Subject: [PATCH 08/13] [MergeBoundingBox] check if robot using subclass not
 is_a.

---
 src/pycram/world_concepts/world_object.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 5fb92d180..34c5edd6d 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -106,7 +106,7 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str]
             self.description.update_description_from_file(self.path)
 
         # if the object is an agent in the belief state
-        if Robot in self.ontology_concept.is_a and not self.world.is_prospection_world:
+        if self.is_a_robot and not self.world.is_prospection_world:
             self._update_world_robot_and_description()
 
         self.id = self._spawn_object_and_get_id()

From 97e6a2edd7268852a117b5d304164273659232ba Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Fri, 17 Jan 2025 11:31:51 +0100
Subject: [PATCH 09/13] [MergeDescriptions] updated doc and removed new object
 in test.

---
 src/pycram/world_concepts/world_object.py | 2 +-
 test/test_object.py                       | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 1832fe8e6..023fb5a56 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -688,7 +688,7 @@ def is_a_robot(self) -> bool:
     def merge(self, other: Object, name: Optional[str] = None, pose: Optional[Pose] = None) -> Object:
         """
         Merge the object with another object. This is done by merging the descriptions of the objects,
-        removing the other object and updating the links and joints of this object.
+        removing the original objects creating a new merged object.
 
         :param other: The object to merge with.
         :param name: The name of the merged object.
diff --git a/test/test_object.py b/test/test_object.py
index b1910decc..66ba527cf 100644
--- a/test/test_object.py
+++ b/test/test_object.py
@@ -191,6 +191,7 @@ def test_merge_bounding_box(self):
             self.assertTrue(isinstance(merged_bbox_mesh, trimesh.parent.Geometry3D))
             self.assertEqual(merged_bbox_mesh.vertices.shape[0], 24 if use_random_events else 16)
             self.assertEqual(merged_bbox_mesh.faces.shape[0], 48 if use_random_events else 24)
+        cereal_2.remove()
 
 
 class GenericObjectTestCase(BulletWorldTestCase):

From 590d7f9e71cae753823606fb4e94a35571f73d32 Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Fri, 17 Jan 2025 14:37:38 +0100
Subject: [PATCH 10/13] [MergeDescriptions] fixed cache problem.

---
 src/pycram/description.py                 |  5 ++-
 src/pycram/object_descriptors/urdf.py     |  6 ++-
 src/pycram/world_concepts/world_object.py | 10 +++--
 test/test_description.py                  | 51 ++++++++++++++---------
 test/test_object.py                       |  3 +-
 5 files changed, 49 insertions(+), 26 deletions(-)

diff --git a/src/pycram/description.py b/src/pycram/description.py
index 73b9cf32a..139cb408c 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -731,7 +731,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
                           child_pose_wrt_parent: Optional[Pose] = None,
-                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+                          in_place: bool = False,
+                          new_description_file: Optional[str] = None) -> Union[ObjectDescription, Self]:
         """
         Merge the description of this object with the description of the other object.
 
@@ -744,6 +745,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
         :param upper_limit: The upper limit of the joint connecting the two objects.
         :param child_pose_wrt_parent: The pose of the child link with respect to the parent link.
         :param in_place: True if the merge should be done in place, False otherwise.
+        :param new_description_file: If given, the new description will be saved to this file, otherwise the new
+            description will be saved in place of the original file.
         :return: The merged object description, could be a new object description if in_place is False else self.
         """
         pass
diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py
index 8023a1e33..b68cb8359 100644
--- a/src/pycram/object_descriptors/urdf.py
+++ b/src/pycram/object_descriptors/urdf.py
@@ -288,7 +288,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
                           child_pose_wrt_parent: Optional[Pose] = None,
-                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+                          in_place: bool = False,
+                          new_description_file: Optional[str] = None) -> Union[ObjectDescription, Self]:
         other_description = other.parsed_description
         if child_pose_wrt_parent is None:
             child_pose_wrt_parent = Pose()
@@ -318,6 +319,9 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                               axis=axis, origin=child_pose_wrt_parent, parent=parent_link,
                               lower_limit=lower_limit, upper_limit=upper_limit)
 
+        if new_description_file is not None:
+            description.xml_path = os.path.join(pathlib.Path(description.xml_path).parent,
+                                                (new_description_file + description.get_file_extension()))
         description.write_description_to_file(description.parsed_description.to_xml_string(), description.xml_path)
         return description
 
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 023fb5a56..792bbd56d 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -685,7 +685,8 @@ def is_a_robot(self) -> bool:
         """
         return issubclass(self.obj_type, pycrap.Robot)
 
-    def merge(self, other: Object, name: Optional[str] = None, pose: Optional[Pose] = None) -> Object:
+    def merge(self, other: Object, name: Optional[str] = None, pose: Optional[Pose] = None,
+              new_description_file: Optional[str] = None) -> Object:
         """
         Merge the object with another object. This is done by merging the descriptions of the objects,
         removing the original objects creating a new merged object.
@@ -693,15 +694,18 @@ def merge(self, other: Object, name: Optional[str] = None, pose: Optional[Pose]
         :param other: The object to merge with.
         :param name: The name of the merged object.
         :param pose: The pose of the merged object.
+        :param new_description_file: The new description file of the merged object.
         :return: The merged object.
         """
         pose = self.pose if pose is None else pose
         child_pose = self.local_transformer.transform_pose(other.pose, self.tf_frame)
-        description = self.description.merge_description(other.description, child_pose_wrt_parent=child_pose)
+        description = self.description.merge_description(other.description, child_pose_wrt_parent=child_pose,
+                                                         new_description_file=new_description_file)
         name = self.name if name is None else name
+        path = self.path if new_description_file is None else description.xml_path
         other.remove()
         self.remove()
-        return Object(name, self.obj_type, self.path, description=description, pose=pose, world=self.world)
+        return Object(name, self.obj_type, path, description=description, pose=pose, world=self.world)
 
     def attach(self,
                child_object: Object,
diff --git a/test/test_description.py b/test/test_description.py
index 1de25c42f..52181d90d 100644
--- a/test/test_description.py
+++ b/test/test_description.py
@@ -1,7 +1,10 @@
 import os.path
 import pathlib
+from copy import deepcopy
 
 from pycram.testing import BulletWorldTestCase
+from pycram.world_concepts.world_object import Object
+from pycrap import Food
 
 
 class DescriptionTest(BulletWorldTestCase):
@@ -38,27 +41,35 @@ def test_generate_description_from_description_file(self):
         self.assertTrue(self.world.cache_manager.is_cached(self.robot.name, self.robot.description))
 
     def test_merge_descriptions(self):
+        cereal_2 = Object("cereal_2", Food, self.cereal.path)
         milk_link_name = self.milk.description.get_root()
         milk_joints_num = len(self.milk.description.joints)
-        cereal_joints_num = len(self.cereal.description.joints)
+        cereal_joints_num = len(cereal_2.description.joints)
         milk_link_num = len(self.milk.description.links)
-        cereal_link_num = len(self.cereal.description.links)
+        cereal_link_num = len(cereal_2.description.links)
+        original_cereal_description = deepcopy(cereal_2.description)
         for in_place in [False, True]:
-            description = self.cereal.description.merge_description(self.milk.description, in_place=in_place)
-            if in_place:
-                self.assertTrue(description is self.cereal.description)
-                self.assertEqual(len(self.cereal.description.joints), milk_joints_num + cereal_joints_num + 1)
-                self.assertEqual(len(self.cereal.description.links), milk_link_num + cereal_link_num)
-                self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
-                                in self.cereal.description.joint_map.keys())
-                self.assertTrue(milk_link_name not in self.cereal.description.link_map.keys())
-                self.assertTrue(f"{self.milk.description.name}_{milk_link_name}" in self.cereal.description.link_map.keys())
-            else:
-                self.assertFalse(self.cereal.description is description)
-                self.assertEqual(len(description.joints), milk_joints_num + cereal_joints_num + 1)
-                self.assertEqual(len(description.links), milk_link_num + cereal_link_num)
-                self.assertTrue(f"{self.cereal.root_link.name}_{self.milk.root_link.name}_joint"
-                                in description.joint_map.keys())
-                self.assertTrue(milk_link_name not in description.link_map.keys())
-                self.assertTrue(
-                    f"{self.milk.description.name}_{milk_link_name}" in description.link_map.keys())
+            for new_description_path in [None, "cereal_milk"]:
+                description = cereal_2.description.merge_description(self.milk.description, in_place=in_place,
+                                                                     new_description_file=new_description_path)
+                if in_place:
+                    self.assertTrue(description is cereal_2.description)
+                    self.assertEqual(len(cereal_2.description.joints), milk_joints_num + cereal_joints_num + 1)
+                    self.assertEqual(len(cereal_2.description.links), milk_link_num + cereal_link_num)
+                    self.assertTrue(f"{cereal_2.root_link.name}_{self.milk.root_link.name}_joint"
+                                    in cereal_2.description.joint_map.keys())
+                    self.assertTrue(milk_link_name not in cereal_2.description.link_map.keys())
+                    self.assertTrue(f"{self.milk.description.name}_{milk_link_name}" in cereal_2.description.link_map.keys())
+                else:
+                    self.assertFalse(cereal_2.description is description)
+                    self.assertEqual(len(description.joints), milk_joints_num + cereal_joints_num + 1)
+                    self.assertEqual(len(description.links), milk_link_num + cereal_link_num)
+                    self.assertTrue(f"{cereal_2.root_link.name}_{self.milk.root_link.name}_joint"
+                                    in description.joint_map.keys())
+                    self.assertTrue(milk_link_name not in description.link_map.keys())
+                    self.assertTrue(
+                        f"{self.milk.description.name}_{milk_link_name}" in description.link_map.keys())
+                if in_place:
+                    cereal_2.description = original_cereal_description
+        cereal_2.remove()
+        self.world.cache_manager.clear_cache()
diff --git a/test/test_object.py b/test/test_object.py
index 66ba527cf..6f2e01e28 100644
--- a/test/test_object.py
+++ b/test/test_object.py
@@ -170,10 +170,11 @@ def test_object_equal(self):
     def test_merge(self):
         cereal = Object("cereal2", Food, "breakfast_cereal.stl")
         milk = Object("milk2", Milk, "milk.stl")
-        cereal_milk = cereal.merge(milk)
+        cereal_milk = cereal.merge(milk, new_description_file="cereal_milk")
         self.assertEqual(len(cereal_milk.links), len(cereal.links) + len(milk.links))
         self.assertEqual(len(cereal_milk.joints), len(cereal.joints) + len(milk.joints) + 1)
         cereal_milk.remove()
+        # self.world.cache_manager.clear_cache()
 
     def test_merge_bounding_box(self):
         cereal_2 = Object("cereal2", Food, "breakfast_cereal.stl",

From 97f0c42f2ff3bb5141ad3d7af75302ee937e87b7 Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Tue, 21 Jan 2025 19:17:10 +0100
Subject: [PATCH 11/13] [MergeDescriptions] updated generic and mjcf
 descriptions.

---
 src/pycram/object_descriptors/generic.py | 3 ++-
 src/pycram/object_descriptors/mjcf.py    | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py
index 480ea07b3..37f89cde0 100644
--- a/src/pycram/object_descriptors/generic.py
+++ b/src/pycram/object_descriptors/generic.py
@@ -116,7 +116,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
                           child_pose_wrt_parent: Optional[Pose] = None,
-                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+                          in_place: bool = False,
+                          new_description_file: Optional[str] = None) -> Union[ObjectDescription, Self]:
         raise NotImplementedError
 
     def load_description(self, path: str) -> Any:
diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py
index 42023853c..1e881a03a 100644
--- a/src/pycram/object_descriptors/mjcf.py
+++ b/src/pycram/object_descriptors/mjcf.py
@@ -363,7 +363,8 @@ def merge_description(self, other: ObjectDescription, parent_link: Optional[str]
                           axis: Optional[Point] = None,
                           lower_limit: Optional[float] = None, upper_limit: Optional[float] = None,
                           child_pose_wrt_parent: Optional[Pose] = None,
-                          in_place: bool = False) -> Union[ObjectDescription, Self]:
+                          in_place: bool = False,
+                          new_description_file: Optional[str] = None) -> Union[ObjectDescription, Self]:
         raise NotImplementedError
 
     @property

From fc7f5a307b5f87d6db1685cc1acb367a67eb527b Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Wed, 29 Jan 2025 17:39:03 +0100
Subject: [PATCH 12/13] [MergeBoundingBoxes] review changes.

---
 src/pycram/datastructures/world.py        | 4 ++--
 src/pycram/world_concepts/world_object.py | 9 ++++-----
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py
index 01f0bb4c0..13d2a9a9e 100644
--- a/src/pycram/datastructures/world.py
+++ b/src/pycram/datastructures/world.py
@@ -412,7 +412,7 @@ def remove_object_from_simulator(self, obj: Object) -> bool:
         """
         pass
 
-    def remove_object(self, obj: Object, remove_from_simulator: bool = True) -> None:
+    def remove_object(self, obj: Object) -> None:
         """
         Remove this object from the current world.
         For the object to be removed it has to be detached from all objects it
@@ -425,7 +425,7 @@ def remove_object(self, obj: Object, remove_from_simulator: bool = True) -> None
 
         obj.detach_all()
 
-        if remove_from_simulator and self.remove_object_from_simulator(obj):
+        if self.remove_object_from_simulator(obj):
             self.objects.remove(obj)
             self.remove_object_from_original_state(obj)
 
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 792bbd56d..97d21cb07 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -27,7 +27,7 @@
 from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 from ..object_descriptors.urdf import ObjectDescription as URDF
 from ..ros.data_types import Time
-from ..ros.logging import logwarn
+from ..ros.logging import logwarn, logerr
 
 try:
     from ..object_descriptors.mjcf import ObjectDescription as MJCF
@@ -379,10 +379,9 @@ def _spawn_object_and_get_id(self) -> int:
             return obj_id
 
         except Exception as e:
-            logging.error(
-                "The File could not be loaded. Please note that the path has to be either a URDF, stl or obj file or"
-                " the name of an URDF string on the parameter server.")
-            logging.error(e)
+            logerr(
+                f"The caught error: {e}, The File could not be loaded. Please note that the path has to be either"
+                f" a URDF, stl or obj file or the name of an URDF string on the parameter server.")
             os.remove(path)
             raise e
 

From 19fd2f2dd6026c2d3fb0a3dad028b6c261655acf Mon Sep 17 00:00:00 2001
From: Abdelrhman Bassiouny <abdelrhman_bassuny@hotmail.com>
Date: Thu, 30 Jan 2025 10:39:04 +0100
Subject: [PATCH 13/13] [MergeBoundingBoxes] fixed issue with
 remove_object_from_simulator and other multiverse related issue.

---
 src/pycram/helper.py                            |  6 ++++--
 .../ur5e_controlled_description.py              | 17 +++++++++++++++--
 src/pycram/world_concepts/world_object.py       |  8 +++-----
 3 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/pycram/helper.py b/src/pycram/helper.py
index 393439d21..0f08a58c4 100644
--- a/src/pycram/helper.py
+++ b/src/pycram/helper.py
@@ -72,14 +72,16 @@ def get_robot_mjcf_path(robot_relative_dir: str, robot_name: str, xml_name: Opti
     except TypeError:
         logwarn("Multiverse resources path not found.")
         return None
-    if multiverse_resources is not None:
+    if multiverse_resources is not None and os.path.exists(robot_folder):
         list_dir = os.listdir(robot_folder)
         if 'mjcf' in list_dir:
             if xml_name in os.listdir(robot_folder + '/mjcf'):
                 return os.path.join(robot_folder, 'mjcf', xml_name)
         elif xml_name in os.listdir(robot_folder):
             return os.path.join(robot_folder, xml_name)
-    return None
+    else:
+        logwarn(f"Robot {robot_name} not found in Multiverse resources.")
+        return None
 
 
 def find_multiverse_resources_path() -> Optional[str]:
diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py
index 125ef1495..1ec8a37ba 100644
--- a/src/pycram/robot_descriptions/ur5e_controlled_description.py
+++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py
@@ -1,5 +1,7 @@
 import os
 
+from typing_extensions import Optional
+
 from ..datastructures.enums import Arms, GripperState
 from ..helper import get_robot_mjcf_path, find_multiverse_resources_path, get_robot_urdf_path_from_multiverse
 from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \
@@ -8,17 +10,28 @@
 from ..ros.logging import logwarn
 
 multiverse_resources = find_multiverse_resources_path()
+
 ROBOT_NAME = "ur5e"
 GRIPPER_NAME = "gripper-2F-85"
 GRIPPER_CMD_TOPIC = "/gripper_command"
 OPEN_VALUE = 0.0
 CLOSE_VALUE = 255.0
-if multiverse_resources is None:
+
+mjcf_filename: Optional[str] = None
+if multiverse_resources is not None:
+    robot_relative_dir = "universal_robot"
+    filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources)
+    mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources)
+
+if mjcf_filename is None:
     logwarn("Could not initialize ur5e description as Multiverse resources path not found.")
 else:
     robot_relative_dir = "universal_robot"
     filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources)
-    mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources)
+    try:
+        mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources)
+    except FileNotFoundError:
+        logwarn(f"Could not find MJCF file for {ROBOT_NAME}.")
     ur5_description = RobotDescription(ROBOT_NAME, "base_link", "", "",
                                        filename, mjcf_path=mjcf_filename, gripper_name=GRIPPER_NAME)
 
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 97d21cb07..afac0a6a7 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -641,15 +641,13 @@ def __repr__(self):
         return self.__class__.__qualname__ + (f"(name={self.name}, object_type={self.obj_type.name},"
                                               f" file_path={self.path}, pose={self.pose}, world={self.world})")
 
-    def remove(self, remove_from_simulator: bool = True) -> None:
+    def remove(self) -> None:
         """
         Remove this object from the World it currently resides in.
         For the object to be removed it has to be detached from all objects it
-        is currently attached to. Then remove this Object from the simulation/world if remove_from_simulator is True.
-
-        :param remove_from_simulator: If True the object will be removed from the simulator.
+        is currently attached to. Then remove this Object from the simulation/world.
         """
-        self.world.remove_object(self, remove_from_simulator=remove_from_simulator)
+        self.world.remove_object(self)
 
     def reset(self, remove_saved_states=False) -> None:
         """