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 9d5578c51..fabd5a68c 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -1,12 +1,17 @@ from __future__ import annotations -import os +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 +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 from .enums import JointType, Shape, VirtualMobileBaseJointName @@ -103,11 +108,127 @@ 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] + + @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]: """ @@ -170,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): """ @@ -201,27 +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) - 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 @@ -229,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. @@ -248,41 +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_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]: + 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/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/description.py b/src/pycram/description.py index 614f688bb..248f800c3 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, Self 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 @@ -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): @@ -357,7 +356,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. @@ -657,8 +656,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 @@ -725,12 +724,40 @@ 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, + 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. + + :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. + :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 + def update_description_from_file(self, path: str) -> None: """ Update the description of this object from the file at the given path. :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/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/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py index d8b8ed4f1..2c343101f 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,19 @@ 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, + 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 d9475d0b2..1e881a03a 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,16 @@ 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, + new_description_file: Optional[str] = None) -> 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 5347f8474..01573574c 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, 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) +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,84 @@ 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, + 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() + + 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() + + 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}' + 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}' + description.add_joint_from_parsed_description(joint) + + # Add the joint between the parent and child link + 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) + + 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 + + def _init_description(self) -> None: + self._init_links() + self._init_joints() def load_description(self, path) -> URDF: with open(path, 'r') as file: @@ -324,18 +400,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. @@ -433,11 +517,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/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py index 1851ada2b..20038a9f0 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, GripperType from ..helper import get_robot_mjcf_path, find_multiverse_resources_path, get_robot_urdf_path_from_multiverse from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ @@ -9,17 +11,28 @@ from ..units import meter 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/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 34e0261bc..7b31d8a7c 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, @@ -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 @@ -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 @@ -114,7 +109,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 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() @@ -387,9 +382,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.") + 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 @@ -646,21 +641,17 @@ 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: """ 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. + is currently attached to. Then remove this Object from the simulation/world. """ - # owlready2.destroy_entity(self.ontology_individual) self.world.remove_object(self) - def reset(self, remove_saved_states=False) -> None: """ Reset the Object to the state it was first spawned in. @@ -694,6 +685,28 @@ 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, + 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. + + :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, + 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, path, description=description, pose=pose, world=self.world) + def attach(self, child_object: Object, parent_link: Optional[str] = None, @@ -1401,15 +1414,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_description.py b/test/test_description.py index e377867e4..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): @@ -36,3 +39,37 @@ 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): + 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(cereal_2.description.joints) + milk_link_num = len(self.milk.description.links) + cereal_link_num = len(cereal_2.description.links) + original_cereal_description = deepcopy(cereal_2.description) + for in_place in [False, True]: + 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_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 e9f39dd44..6f2e01e28 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 @@ -165,6 +167,33 @@ 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, 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", + 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) + cereal_2.remove() + class GenericObjectTestCase(BulletWorldTestCase): @@ -182,7 +211,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()))