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: """