From 75c16a04fcad66aeed875664e487657cf8854ae5 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 4 Oct 2024 18:41:57 +0200 Subject: [PATCH 1/9] [WorldObject] Added callbacks for when objects are added or removed. --- src/pycram/datastructures/world.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index d9c16e5e9..016f80a5b 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -122,6 +122,24 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca self.original_state_id = self.save_state() + self.on_add_object_callbacks: List[Callable[[Object], None]] = [] + + def add_callback_on_add_object(self, callback: Callable[[Object], None]) -> None: + """ + Add a callback that is called when an object is added to the world. + + :param callback: The callback. + """ + self.on_add_object_callbacks.append(callback) + + def remove_callback_on_add_object(self, callback: Callable[[Object], None]) -> None: + """ + Remove a callback that is called when an object is added to the world. + + :param callback: The callback. + """ + self.on_add_object_callbacks.remove(callback) + @classmethod def get_cache_dir(cls) -> str: """ @@ -139,6 +157,16 @@ def add_object(self, obj: Object) -> None: self.objects.append(obj) self.add_object_to_original_state(obj) self.object_lock.release() + self.invoke_on_add_object_callbacks(obj) + + def invoke_on_add_object_callbacks(self, obj: Object) -> None: + """ + Invoke the object added callbacks. + + :param obj: The object. + """ + for callback in self.on_add_object_callbacks: + callback(obj) @property def robot_description(self) -> RobotDescription: From b2564c919dbc53437a4720c77da66c02d9e6667a Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Mon, 7 Oct 2024 15:48:32 +0200 Subject: [PATCH 2/9] [MeshLoading] added the ability to add a transformation to apply to the mesh. --- src/pycram/cache_manager.py | 10 ++++++++-- src/pycram/datastructures/world.py | 7 +++++-- src/pycram/description.py | 11 +++++++++-- src/pycram/world_concepts/world_object.py | 6 ++++-- 4 files changed, 26 insertions(+), 8 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 3e6a9889d..d2cec0eff 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -3,10 +3,12 @@ import pathlib import shutil +import numpy as np from typing_extensions import List, TYPE_CHECKING, Optional if TYPE_CHECKING: from .description import ObjectDescription + from .datastructures.pose import Transform class CacheManager: @@ -50,7 +52,8 @@ def delete_cache_dir(self): def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, object_description: 'ObjectDescription', object_name: str, - scale_mesh: Optional[float] = None) -> str: + scale_mesh: Optional[float] = None, + mesh_transform: Optional['Transform'] = None) -> str: """ Check if the file is already in the cache directory, if not preprocess and save in the cache. @@ -60,6 +63,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, :param object_description: The object description of the file. :param object_name: The name of the object. :param scale_mesh: The scale of the mesh. + :param mesh_transform: The transformation matrix to apply to the mesh. :return: The path of the cached file. """ path_object = pathlib.Path(path) @@ -73,7 +77,9 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, if not self.is_cached(path, object_description) or ignore_cached_files: # if file is not yet cached preprocess the description file and save it in the cache directory. path = self.look_for_file_in_data_dir(path_object) - object_description.generate_description_from_file(path, object_name, extension, cache_path, scale_mesh) + object_description.original_path = path + object_description.generate_description_from_file(path, object_name, extension, cache_path, + scale_mesh, mesh_transform) return cache_path diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 016f80a5b..3cd66b09e 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -288,7 +288,8 @@ def _sync_prospection_world(self): def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached_files: bool, description: ObjectDescription, name: str, - scale_mesh: Optional[float] = None) -> str: + scale_mesh: Optional[float] = None, + mesh_transform: Optional[Transform] = None) -> str: """ Update the cache directory with the given object. @@ -297,9 +298,11 @@ def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached :param description: The object description. :param name: The name of the object. :param scale_mesh: The scale of the mesh. + :param mesh_transform: The mesh transform to apply to the mesh. :return: The path of the cached object. """ - return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name, scale_mesh) + return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name, + scale_mesh, mesh_transform) @property def simulation_time_step(self): diff --git a/src/pycram/description.py b/src/pycram/description.py index bd3002ed7..b6b4d600d 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -5,6 +5,7 @@ import pathlib from abc import ABC, abstractmethod +import numpy as np import rospy import trimesh from geometry_msgs.msg import Point, Quaternion @@ -620,6 +621,7 @@ def __init__(self, path: Optional[str] = None): self._joints: Optional[List[JointDescription]] = 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.update_description_from_file(path) @@ -737,7 +739,8 @@ def load_description(self, path: str) -> Any: pass def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str, - scale_mesh: Optional[float] = None) -> None: + scale_mesh: Optional[float] = None, + mesh_transform: Optional[Transform] = None) -> None: """ Generate and preprocess the description from the file at the given path and save the preprocessed description. The generated description will be saved at the given save path. @@ -747,15 +750,19 @@ def generate_description_from_file(self, path: str, name: str, extension: str, s :param extension: The file extension of the file to preprocess. :param save_path: The path to save the generated description file. :param scale_mesh: The scale of the mesh. + :param mesh_transform: The transformation matrix to apply to the mesh. :raises ObjectDescriptionNotFound: If the description file could not be found/read. """ if extension in self.mesh_extensions: if extension == ".ply": mesh = trimesh.load(path) - path = path.replace(extension, ".obj") if scale_mesh is not None: mesh.apply_scale(scale_mesh) + if mesh_transform is not None: + transform = mesh_transform.get_homogeneous_matrix() + mesh.apply_transform(transform) + path = path.replace(extension, ".obj") mesh.export(path) self.generate_from_mesh_file(path, name, save_path=save_path) elif extension == self.get_file_extension(): diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 4975fa983..773400e5e 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -55,7 +55,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, world: Optional[World] = None, color: Color = Color(), ignore_cached_files: bool = False, - scale_mesh: Optional[float] = None): + scale_mesh: Optional[float] = None, + mesh_transform: Optional[Transform] = None): """ The constructor loads the description file into the given World, if no World is specified the :py:attr:`~World.current_world` will be used. It is also possible to load .obj and .stl file into the World. @@ -93,7 +94,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, if path is not None: self.path = self.world.preprocess_object_file_and_get_its_cache_path(path, ignore_cached_files, self.description, self.name, - scale_mesh=scale_mesh) + scale_mesh=scale_mesh, + mesh_transform=mesh_transform) self.description.update_description_from_file(self.path) From 4fa042a7496f1ce907dfa7d6dad8a924a0ff9e36 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 9 Oct 2024 15:27:47 +0200 Subject: [PATCH 3/9] [MeshAndObject] Added methods that can directly calculate the bounding box depending on the visual shape of the link. Corrected multiverse mesh visual shape parsing. Added methods to get mesh path and file name. --- src/pycram/datastructures/dataclasses.py | 47 +++++++++++++++++++++-- src/pycram/datastructures/pose.py | 4 +- src/pycram/description.py | 44 +++++++++++++++++---- src/pycram/object_descriptors/mjcf.py | 3 +- src/pycram/world_concepts/world_object.py | 16 +++++++- src/pycram/worlds/multiverse.py | 23 +++++------ test/test_multiverse.py | 29 +++++++++++++- 7 files changed, 140 insertions(+), 26 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 040189ce3..d0058da31 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -4,10 +4,11 @@ from copy import deepcopy, copy from dataclasses import dataclass -from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING +import numpy as np +from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Iterable, Sequence from .enums import JointType, Shape, VirtualMobileBaseJointName -from .pose import Pose, Point +from .pose import Pose, Point, Transform from ..validation.error_checkers import calculate_joint_position_error, is_error_acceptable if TYPE_CHECKING: @@ -100,7 +101,7 @@ class AxisAlignedBoundingBox: max_z: float @classmethod - def from_min_max(cls, min_point: List[float], max_point: List[float]): + 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. @@ -109,6 +110,16 @@ def from_min_max(cls, min_point: List[float], max_point: List[float]): """ return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2]) + def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox': + """ + Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box. + + :param transform: The transformation to apply + :return: The transformed axis-aligned bounding box + """ + transformed_points = transform.apply_transform_to_array_of_points(np.array(self.get_min_max())) + return AxisAlignedBoundingBox.from_min_max(transformed_points[0], transformed_points[1]) + def get_min_max_points(self) -> Tuple[Point, Point]: """ :return: The axis-aligned bounding box as a tuple of minimum and maximum points @@ -196,6 +207,13 @@ def visual_geometry_type(self) -> Shape: """ pass + @property + def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis-aligned bounding box of the visual shape. + """ + raise NotImplementedError + @dataclass class BoxVisualShape(VisualShape): @@ -215,6 +233,14 @@ def visual_geometry_type(self) -> Shape: def size(self) -> List[float]: return self.half_extents + @property + def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis-aligned bounding box of the box visual shape. + """ + return AxisAlignedBoundingBox(-self.half_extents[0], -self.half_extents[1], -self.half_extents[2], + self.half_extents[0], self.half_extents[1], self.half_extents[2]) + @dataclass class SphereVisualShape(VisualShape): @@ -230,6 +256,13 @@ def shape_data(self) -> Dict[str, float]: def visual_geometry_type(self) -> Shape: return Shape.SPHERE + @property + def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis-aligned bounding box of the sphere visual shape. + """ + return AxisAlignedBoundingBox(-self.radius, -self.radius, -self.radius, self.radius, self.radius, self.radius) + @dataclass class CapsuleVisualShape(VisualShape): @@ -246,6 +279,14 @@ def shape_data(self) -> Dict[str, float]: def visual_geometry_type(self) -> Shape: return Shape.CAPSULE + @property + def axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :return: The axis-aligned bounding box of the capsule visual shape. + """ + return AxisAlignedBoundingBox(-self.radius, -self.radius, -self.length / 2, + self.radius, self.radius, self.length / 2) + @dataclass class CylinderVisualShape(CapsuleVisualShape): diff --git a/src/pycram/datastructures/pose.py b/src/pycram/datastructures/pose.py index 490a56e9f..e582d79ee 100644 --- a/src/pycram/datastructures/pose.py +++ b/src/pycram/datastructures/pose.py @@ -379,8 +379,8 @@ def apply_transform_to_array_of_points(self, points: np.ndarray) -> np.ndarray: homogeneous_transform = self.get_homogeneous_matrix() # add the homogeneous coordinate, by adding a column of ones to the position vectors, becoming 4xN matrix homogenous_points = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1).T - rays_end_positions = homogeneous_transform @ homogenous_points - return rays_end_positions[:3, :].T + transformed_points = homogeneous_transform @ homogenous_points + return transformed_points[:3, :].T def get_homogeneous_matrix(self) -> np.ndarray: """ diff --git a/src/pycram/description.py b/src/pycram/description.py index b6b4d600d..255a5cca0 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -11,7 +11,8 @@ from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated -from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape +from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ + MeshVisualShape from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity @@ -220,6 +221,41 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self._current_pose: Optional[Pose] = None self.update_pose() + def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + """ + :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. + """ + try: + return self.world.get_link_axis_aligned_bounding_box(self) + except NotImplementedError: + if isinstance(self.geometry, MeshVisualShape): + mesh_path = self.get_mesh_path() + mesh = trimesh.load(mesh_path) + min_bound, max_bound = mesh.bounds + return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound).get_transformed_box(self.transform) + else: + return self.geometry.axis_aligned_bounding_box.get_transformed_box(self.transform) + + def get_mesh_path(self) -> str: + """ + :return: The path of the mesh file of this link if the geometry is a mesh, otherwise raise a ValueError. + """ + mesh_filename = self.get_mesh_filename() + return self.world.cache_manager.look_for_file_in_data_dir(pathlib.Path(mesh_filename)) + + def get_mesh_filename(self) -> str: + """ + :return: The mesh file name of this link if the geometry is a mesh, otherwise raise a ValueError. + :raises ValueError: If the geometry is not a mesh or if the link has no geometry. + """ + if self.geometry is None: + raise ValueError("The link has no geometry.") + if not isinstance(self.geometry, MeshVisualShape): + raise ValueError(f"The link geometry is not of type {MeshVisualShape.__name__}," + f" it is {type(self.geometry)}.") + return self.geometry.file_name + def set_pose(self, pose: Pose) -> None: """ Set the pose of this link to the given pose. @@ -339,12 +375,6 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose: """ return self.local_transformer.transform_pose(self.pose, link.tf_frame) - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: - """ - :return: An AxisAlignedBoundingBox object with the axis aligned bounding box of this link. - """ - return self.world.get_link_axis_aligned_bounding_box(self) - @property def position(self) -> Point: """ diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index d4200db1f..02aafb6da 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -55,7 +55,8 @@ def _get_visual_shape(mjcf_geometry) -> Union[VisualShape, None]: if mjcf_geometry.type == MJCFGeomType.SPHERE.value: return SphereVisualShape(Color(), [0, 0, 0], mjcf_geometry.size[0]) if mjcf_geometry.type == MJCFGeomType.MESH.value: - return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.scale, mjcf_geometry.filename) + mesh_filename = mjcf_geometry.mesh.file.prefix + mjcf_geometry.mesh.file.extension + return MeshVisualShape(Color(), [0, 0, 0], mjcf_geometry.mesh.scale, mesh_filename) return None @property diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 773400e5e..9a600ae88 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -116,6 +116,17 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, self.world.add_object(self) + def get_mesh_path(self) -> str: + """ + Get the path to the mesh file of the object. + + :return: The path to the mesh file. + """ + if self.has_one_link: + return self.root_link.get_mesh_path() + else: + raise ValueError("The object has more than one link, therefore the mesh path cannot be determined.") + def _resolve_description(self, path: Optional[str] = None, description: Optional[ObjectDescription] = None) -> None: """ Find the correct description type of the object and initialize it and set the description of this object to it. @@ -1344,7 +1355,10 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: :return: The axis aligned bounding box of this object. """ - return self.world.get_object_axis_aligned_bounding_box(self) + if self.has_one_link: + return self.links[self.description.get_root()].get_axis_aligned_bounding_box() + else: + return self.world.get_object_axis_aligned_bounding_box(self) def get_base_origin(self) -> Pose: """ diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index fd174155b..52dbb98c5 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -3,18 +3,19 @@ import numpy as np import rospy +import trimesh from tf.transformations import quaternion_matrix from typing_extensions import List, Dict, Optional, Union, Tuple from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from ..config.multiverse_conf import MultiverseConfig -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, MeshVisualShape from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from ..datastructures.pose import Pose from ..datastructures.world import World -from ..description import Link, Joint, ObjectDescription +from ..description import Link, Joint from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescription from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz @@ -327,7 +328,7 @@ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[J :param joint_positions: The dictionary of joints and positions. """ controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -621,30 +622,30 @@ def restore_physics_simulator_state(self, state_id: int) -> None: self.api_requester.load(self.saved_simulator_states[state_id]) def set_link_color(self, link: Link, rgba_color: Color): - logging.warning("set_link_color is not implemented in Multiverse") + rospy.logwarn("set_link_color is not implemented in Multiverse") def get_link_color(self, link: Link) -> Color: - logging.warning("get_link_color is not implemented in Multiverse") + rospy.logwarn("get_link_color is not implemented in Multiverse") return Color() def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - logging.warning("get_colors_of_object_links is not implemented in Multiverse") + rospy.logwarn("get_colors_of_object_links is not implemented in Multiverse") return {} def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - logging.error("get_object_axis_aligned_bounding_box is not implemented in Multiverse") + rospy.logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse") raise NotImplementedError def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - logging.error("get_link_axis_aligned_bounding_box is not implemented in Multiverse") + rospy.logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError def set_realtime(self, real_time: bool) -> None: - logging.warning("set_realtime is not implemented as an API in Multiverse, it is configured in the" - "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") + rospy.logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the" + "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") def set_gravity(self, gravity_vector: List[float]) -> None: - logging.warning("set_gravity is not implemented in Multiverse") + rospy.logwarn("set_gravity is not implemented in Multiverse") def check_object_exists(self, obj: Object) -> bool: """ diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4c1426211..102fb2dd1 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -7,7 +7,7 @@ from tf.transformations import quaternion_from_euler, quaternion_multiply from typing_extensions import Optional, List -from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint +from pycram.datastructures.dataclasses import ContactPointsList, ContactPoint, AxisAlignedBoundingBox from pycram.datastructures.enums import ObjectType, Arms, JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescriptionManager @@ -53,6 +53,33 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.remove_all_objects() + def test_get_axis_aligned_bounding_box_for_one_link_object(self): + position = [1, 1, 0.1] + milk = self.spawn_milk(position) + aabb = milk.get_axis_aligned_bounding_box() + self.assertIsInstance(aabb, AxisAlignedBoundingBox) + min_p_1, max_p_1 = aabb.get_min_max() + width = max_p_1[0] - min_p_1[0] + length = max_p_1[1] - min_p_1[1] + height = max_p_1[2] - min_p_1[2] + self.assertTrue(width > 0) + self.assertTrue(length > 0) + self.assertTrue(height > 0) + # Move the object and check if the bounding box is updated correctly + position_shift = 1 + milk.set_position([position[0] + position_shift, position[1] + position_shift, 0.1]) + aabb = milk.get_axis_aligned_bounding_box() + min_p_2, max_p_2 = aabb.get_min_max() + width_2 = max_p_2[0] - min_p_2[0] + length_2 = max_p_2[1] - min_p_2[1] + height_2 = max_p_2[2] - min_p_2[2] + self.assertAlmostEqual(width, width_2, delta=0.001) + self.assertAlmostEqual(length, length_2, delta=0.001) + self.assertAlmostEqual(height, height_2, delta=0.001) + for i in range(3): + self.assertAlmostEqual(min_p_1[0] + position_shift, min_p_2[0], delta=0.001) + self.assertAlmostEqual(max_p_1[0] + position_shift, max_p_2[0], delta=0.001) + def test_spawn_mesh_object(self): milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) From fbd2134925a29b6f5723c512637cb2a49b6556c0 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 9 Oct 2024 15:41:25 +0200 Subject: [PATCH 4/9] [MeshAndObject] Added new failures for wrong or no geometry for link. --- src/pycram/description.py | 17 +++++++++-------- src/pycram/failures.py | 10 ++++++++++ 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 255a5cca0..0576ba3a1 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -16,7 +16,7 @@ from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity -from .failures import ObjectDescriptionNotFound +from .failures import ObjectDescriptionNotFound, LinkHasNoGeometry, LinkGeometryHasNoMesh from .local_transformer import LocalTransformer if TYPE_CHECKING: @@ -246,15 +246,16 @@ def get_mesh_path(self) -> str: def get_mesh_filename(self) -> str: """ - :return: The mesh file name of this link if the geometry is a mesh, otherwise raise a ValueError. - :raises ValueError: If the geometry is not a mesh or if the link has no geometry. + :return: The mesh file name of this link if the geometry is a mesh, otherwise raise a LinkGeometryHasNoMesh. + :raises LinkHasNoGeometry: If the link has no geometry. + :raises LinkGeometryHasNoMesh: If the geometry is not a mesh. """ if self.geometry is None: - raise ValueError("The link has no geometry.") - if not isinstance(self.geometry, MeshVisualShape): - raise ValueError(f"The link geometry is not of type {MeshVisualShape.__name__}," - f" it is {type(self.geometry)}.") - return self.geometry.file_name + raise LinkHasNoGeometry(self.name) + if isinstance(self.geometry, MeshVisualShape): + return self.geometry.file_name + else: + raise LinkGeometryHasNoMesh(self.name, type(self.geometry).__name__) def set_pose(self, pose: Pose) -> None: """ diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 736adf8ee..422da36b3 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -469,3 +469,13 @@ class UnsupportedJointType(Exception): def __init__(self, joint_type: 'JointType'): super().__init__(f"Unsupported joint type: {joint_type}") + +class LinkHasNoGeometry(Exception): + def __init__(self, link_name: str): + super().__init__(f"Link {link_name} has no geometry.") + + +class LinkGeometryHasNoMesh(Exception): + def __init__(self, link_name: str, geometry_type: str): + super().__init__(f"Link {link_name} geometry with type {geometry_type} has no mesh.") + From 10932533d9ba37e79a43725e3ba14f1661d5c04b Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 9 Oct 2024 15:50:30 +0200 Subject: [PATCH 5/9] [MeshAndObject] modified doc of method. --- src/pycram/description.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/description.py b/src/pycram/description.py index 0576ba3a1..07f3a927d 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -239,7 +239,7 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: def get_mesh_path(self) -> str: """ - :return: The path of the mesh file of this link if the geometry is a mesh, otherwise raise a ValueError. + :return: The path of the mesh file of this link if the geometry is a mesh. """ mesh_filename = self.get_mesh_filename() return self.world.cache_manager.look_for_file_in_data_dir(pathlib.Path(mesh_filename)) From 0db8e9df2dd9d8125bda30eec254052ba478cdbf Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Wed, 9 Oct 2024 19:08:55 +0200 Subject: [PATCH 6/9] [MeshAndObject] Added rotated bounding box. --- src/pycram/costmaps.py | 2 +- src/pycram/datastructures/dataclasses.py | 122 +++++++++++++++++++--- src/pycram/datastructures/world.py | 31 ++++-- src/pycram/description.py | 26 +++-- src/pycram/world_concepts/world_object.py | 17 ++- test/test_links.py | 28 +++++ test/test_multiverse.py | 3 +- 7 files changed, 190 insertions(+), 39 deletions(-) diff --git a/src/pycram/costmaps.py b/src/pycram/costmaps.py index ad34677e6..de36892e8 100644 --- a/src/pycram/costmaps.py +++ b/src/pycram/costmaps.py @@ -785,7 +785,7 @@ def get_aabb_for_link(self) -> AxisAlignedBoundingBox: link_pose_trans = self.link.transform inverse_trans = link_pose_trans.invert() prospection_object.set_orientation(inverse_trans.to_pose()) - return self.link.get_axis_aligned_bounding_box() + return self.link.get_bounding_box() class AlgebraicSemanticCostmap(SemanticCostmap): diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d0058da31..6dab0ebe4 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -5,7 +5,7 @@ from dataclasses import dataclass import numpy as np -from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Iterable, Sequence +from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING, Sequence from .enums import JointType, Shape, VirtualMobileBaseJointName from .pose import Pose, Point, Transform @@ -89,7 +89,7 @@ def get_rgb(self) -> List[float]: @dataclass -class AxisAlignedBoundingBox: +class BoundingBox: """ Dataclass for storing an axis-aligned bounding box. """ @@ -100,25 +100,24 @@ class AxisAlignedBoundingBox: max_y: float max_z: float - @classmethod - def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float]): + def get_points_list(self) -> List[List[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: The points of the bounding box as a list of lists of floats. """ - return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2]) + return [[point.x, point.y, point.z] for point in self.get_points()] - def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox': + def get_points(self) -> List[Point]: """ - Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box. - - :param transform: The transformation to apply - :return: The transformed axis-aligned bounding box + :return: The points of the bounding box as a list of Point instances. """ - transformed_points = transform.apply_transform_to_array_of_points(np.array(self.get_min_max())) - return AxisAlignedBoundingBox.from_min_max(transformed_points[0], transformed_points[1]) + return [Point(self.min_x, self.min_y, self.min_z), + Point(self.min_x, self.min_y, self.max_z), + Point(self.min_x, self.max_y, self.min_z), + Point(self.min_x, self.max_y, self.max_z), + Point(self.max_x, self.min_y, self.min_z), + Point(self.max_x, self.min_y, self.max_z), + Point(self.max_x, self.max_y, self.min_z), + Point(self.max_x, self.max_y, self.max_z)] def get_min_max_points(self) -> Tuple[Point, Point]: """ @@ -157,6 +156,96 @@ def get_max(self) -> List[float]: return [self.max_x, self.max_y, self.max_z] +@dataclass +class AxisAlignedBoundingBox(BoundingBox): + + def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox': + """ + Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box. + + :param transform: The transformation to apply + :return: The transformed axis-aligned bounding box + """ + transformed_points = transform.apply_transform_to_array_of_points(np.array(self.get_min_max())) + min_p = [min(transformed_points[:, i]) for i in range(3)] + max_p = [max(transformed_points[:, i]) for i in range(3)] + return AxisAlignedBoundingBox.from_min_max(min_p, max_p) + + @classmethod + def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float]): + """ + Set the axis-aligned bounding box from a minimum and maximum point. + + :param min_point: The minimum point + :param max_point: The maximum point + """ + return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2]) + + +@dataclass +class RotatedBoundingBox(BoundingBox): + """ + Dataclass for storing a rotated bounding box. + """ + + def __init__(self, min_x: float, min_y: float, min_z: float, max_x: float, max_y: float, max_z: float, + transform: Transform, points: Optional[List[Point]] = None): + self.min_x, self.min_y, self.min_z = min_x, min_y, min_z + self.max_x, self.max_y, self.max_z = max_x, max_y, max_z + self.transform: Transform = transform + self._points: Optional[List[Point]] = points + + @classmethod + def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], transform: Transform): + """ + Set the rotated bounding box from a minimum, maximum point, and a transformation. + + :param min_point: The minimum point + :param max_point: The maximum point + :param transform: The transformation + """ + return cls(min_point[0], min_point[1], min_point[2], max_point[0], max_point[1], max_point[2], transform) + + @classmethod + def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBoundingBox, + transform: Transform) -> 'RotatedBoundingBox': + """ + Set the rotated bounding box from an axis-aligned bounding box and a transformation. + + :param axis_aligned_bounding_box: The axis-aligned bounding box. + :param transform: The transformation. + """ + return cls(axis_aligned_bounding_box.min_x, axis_aligned_bounding_box.min_y, axis_aligned_bounding_box.min_z, + axis_aligned_bounding_box.max_x, axis_aligned_bounding_box.max_y, axis_aligned_bounding_box.max_z, + transform) + + def get_points_list(self) -> List[List[float]]: + """ + :return: The points of the rotated bounding box as a list of lists of floats. + """ + return [[point.x, point.y, point.z] for point in self.get_points()] + + def get_points(self, transform: Optional[Transform] = None) -> List[Point]: + """ + :param transform: The transformation to apply to the points, if None the stored transformation is used. + :return: The points of the rotated bounding box. + """ + if (self._points is None) or (transform is not None): + if transform is not None: + self.transform = transform + points_array = np.array([[self.min_x, self.min_y, self.min_z], + [self.min_x, self.min_y, self.max_z], + [self.min_x, self.max_y, self.min_z], + [self.min_x, self.max_y, self.max_z], + [self.max_x, self.min_y, self.min_z], + [self.max_x, self.min_y, self.max_z], + [self.max_x, self.max_y, self.min_z], + [self.max_x, self.max_y, self.max_z]]) + transformed_points = self.transform.apply_transform_to_array_of_points(points_array).tolist() + self._points = [Point(*point) for point in transformed_points] + return self._points + + @dataclass class CollisionCallbacks: """ @@ -531,6 +620,7 @@ class ContactPointsList(list): """ A list of contact points. """ + def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]: """ Return the links that are not in the current points list but were in the initial points list. diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index 3cd66b09e..2a2c4d397 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -19,7 +19,7 @@ SphereVisualShape, CapsuleVisualShape, PlaneVisualShape, MeshVisualShape, ObjectState, WorldState, ClosestPointsList, - ContactPointsList, VirtualMobileBaseJoints) + ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox) from ..datastructures.enums import JointType, ObjectType, WorldMode, Arms from ..datastructures.pose import Pose, Transform from ..datastructures.world_entity import StateEntity @@ -837,8 +837,6 @@ def set_link_color(self, link: Link, rgba_color: Color): @abstractmethod def get_link_color(self, link: Link) -> Color: """ - This method returns the rgba_color of this link. - :param link: The link for which the rgba_color should be returned. :return: The rgba_color as Color object with RGBA values between 0 and 1. """ @@ -847,32 +845,45 @@ def get_link_color(self, link: Link) -> Color: @abstractmethod def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: """ - Get the RGBA colors of each link in the object as a dictionary from link name to rgba_color. - :param obj: The object - :return: A dictionary with link names as keys and a Color object for each link as value. + :return: The RGBA colors of each link in the object as a dictionary from link name to rgba_color. """ pass @abstractmethod def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: """ - Return the axis aligned bounding box of this object. The return of this method are two points in + :param obj: The object for which the bounding box should be returned. + :return: the axis aligned bounding box of this object. The return of this method are two points in world coordinate frame which define a bounding box. + """ + pass + def get_object_rotated_bounding_box(self, obj: Object) -> RotatedBoundingBox: + """ :param obj: The object for which the bounding box should be returned. - :return: AxisAlignedBoundingBox object containing the min and max points of the bounding box. + :return: the rotated bounding box of this object. The return of this method are two points in + world coordinate frame which define a bounding box. """ - pass + raise NotImplementedError @abstractmethod def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: """ - Return the axis aligned bounding box of the link. The return of this method are two points in + :param link: The link for which the bounding box should be returned. + :return: The axis aligned bounding box of the link. The return of this method are two points in world coordinate frame which define a bounding box. """ pass + def get_link_rotated_bounding_box(self, link: Link) -> RotatedBoundingBox: + """ + :param link: The link for which the bounding box should be returned. + :return: The rotated bounding box of the link. The return of this method are two points in + world coordinate frame which define a bounding box. + """ + raise NotImplementedError + @abstractmethod def set_realtime(self, real_time: bool) -> None: """ diff --git a/src/pycram/description.py b/src/pycram/description.py index 07f3a927d..a5ad83faa 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -5,14 +5,13 @@ import pathlib from abc import ABC, abstractmethod -import numpy as np import rospy import trimesh from geometry_msgs.msg import Point, Quaternion from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \ - MeshVisualShape + MeshVisualShape, RotatedBoundingBox from .datastructures.enums import JointType from .datastructures.pose import Pose, Transform from .datastructures.world_entity import WorldEntity @@ -221,21 +220,32 @@ def __init__(self, _id: int, link_description: LinkDescription, obj: Object): self._current_pose: Optional[Pose] = None self.update_pose() - def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: + def get_bounding_box(self, rotated: bool = False) -> Union[AxisAlignedBoundingBox, RotatedBoundingBox]: """ - :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. + :param rotated: If True, return the rotated bounding box, otherwise the axis-aligned bounding box. + :return: The axis-aligned or rotated 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. """ try: - return self.world.get_link_axis_aligned_bounding_box(self) + if rotated: + return self.world.get_link_rotated_bounding_box(self) + else: + return self.world.get_link_axis_aligned_bounding_box(self) except NotImplementedError: if isinstance(self.geometry, MeshVisualShape): mesh_path = self.get_mesh_path() mesh = trimesh.load(mesh_path) min_bound, max_bound = mesh.bounds - return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound).get_transformed_box(self.transform) + if rotated: + return RotatedBoundingBox.from_min_max(min_bound, max_bound, self.transform) + else: + return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound).get_transformed_box(self.transform) else: - return self.geometry.axis_aligned_bounding_box.get_transformed_box(self.transform) + bounding_box = self.geometry.axis_aligned_bounding_box + if rotated: + return RotatedBoundingBox.from_axis_aligned_bounding_box(bounding_box, self.transform) + else: + return bounding_box.get_transformed_box(self.transform) def get_mesh_path(self) -> str: """ diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 9a600ae88..f5d2fb58b 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -12,7 +12,7 @@ from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, AxisAlignedBoundingBox, VisualShape, ClosestPointsList, - ContactPointsList) + ContactPointsList, RotatedBoundingBox) from ..datastructures.enums import ObjectType, JointType from ..datastructures.pose import Pose, Transform from ..datastructures.world import World @@ -500,7 +500,7 @@ def get_link_axis_aligned_bounding_box(self, link_name: str) -> AxisAlignedBound :param link_name: The name of the link. :return: The axis aligned bounding box of the link. """ - return self.links[link_name].get_axis_aligned_bounding_box() + return self.links[link_name].get_bounding_box() def get_transform_between_links(self, from_link: str, to_link: str) -> Transform: """ @@ -1356,10 +1356,21 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: :return: The axis aligned bounding box of this object. """ if self.has_one_link: - return self.links[self.description.get_root()].get_axis_aligned_bounding_box() + return self.links[self.description.get_root()].get_bounding_box() else: return self.world.get_object_axis_aligned_bounding_box(self) + def get_rotated_bounding_box(self) -> RotatedBoundingBox: + """ + Return the rotated bounding box of this object. + + :return: The rotated bounding box of this object. + """ + if self.has_one_link: + return self.links[self.description.get_root()].get_bounding_box(rotated=True) + else: + return self.world.get_object_rotated_bounding_box(self) + def get_base_origin(self) -> Pose: """ Return the origin of the base/bottom of this object. diff --git a/test/test_links.py b/test/test_links.py index 695e670c1..9cb01bbaa 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,9 +1,37 @@ +import numpy as np +from matplotlib import pyplot as plt +from tf.transformations import quaternion_from_euler + from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.dataclasses import Color +from pycram.datastructures.pose import Pose class TestLinks(BulletWorldTestCase): + def test_rotated_bounding_box(self): + self.milk.set_pose(Pose([1, 1, 1], quaternion_from_euler(np.pi/4, 0, 0).tolist())) + aabb = self.milk.get_axis_aligned_bounding_box() + aabb_points = np.array(aabb.get_points_list()) + rbb = self.milk.get_rotated_bounding_box() + rot_points = np.array(rbb.get_points_list()) + plot = False + if plot: + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + + ax.scatter(rot_points[:, 0], rot_points[:, 1], rot_points[:, 2], c='r', marker='o') + ax.scatter(aabb_points[:, 0], aabb_points[:, 1], aabb_points[:, 2], c='b', marker='o') + + 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() + def test_add_constraint(self): milk_link = self.milk.root_link robot_link = self.robot.root_link diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 102fb2dd1..bd348c38f 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -55,7 +55,8 @@ def tearDown(self): def test_get_axis_aligned_bounding_box_for_one_link_object(self): position = [1, 1, 0.1] - milk = self.spawn_milk(position) + milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1], + quaternion_from_euler(np.pi/4, 0, 0).tolist())) aabb = milk.get_axis_aligned_bounding_box() self.assertIsInstance(aabb, AxisAlignedBoundingBox) min_p_1, max_p_1 = aabb.get_min_max() From b6bb6961bc5b8c3d2a7edf1ad9aba1e93baf49c6 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 18:03:02 +0200 Subject: [PATCH 7/9] [MeshAndObject] correcting the requested changes in review. --- src/pycram/cache_manager.py | 15 ++-- src/pycram/datastructures/dataclasses.py | 90 ++++++++++++++---------- src/pycram/worlds/multiverse.py | 22 +++--- 3 files changed, 72 insertions(+), 55 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index d2cec0eff..29bc5c2ba 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -1,11 +1,14 @@ +from __future__ import annotations + import glob import os import pathlib import shutil -import numpy as np from typing_extensions import List, TYPE_CHECKING, Optional +from .ros.logging import loginfo + if TYPE_CHECKING: from .description import ObjectDescription from .datastructures.pose import Transform @@ -51,9 +54,9 @@ def delete_cache_dir(self): shutil.rmtree(self.cache_dir) def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, - object_description: 'ObjectDescription', object_name: str, + object_description: ObjectDescription, object_name: str, scale_mesh: Optional[float] = None, - mesh_transform: Optional['Transform'] = None) -> str: + mesh_transform: Optional[Transform] = None) -> str: """ Check if the file is already in the cache directory, if not preprocess and save in the cache. @@ -96,7 +99,7 @@ def look_for_file_in_data_dir(self, path_object: pathlib.Path) -> str: for file in glob.glob(str(data_path), recursive=True): file_path = pathlib.Path(file) if file_path.name == name: - print(f"Found file {name} in {file_path}") + loginfo(f"Found file {name} in {file_path}") return str(file_path) raise FileNotFoundError( @@ -109,7 +112,7 @@ def create_cache_dir_if_not_exists(self): if not pathlib.Path(self.cache_dir).exists(): os.mkdir(self.cache_dir) - def is_cached(self, path: str, object_description: 'ObjectDescription') -> bool: + def is_cached(self, path: str, object_description: ObjectDescription) -> bool: """ Check if the file in the given path is already cached or if there is already a cached file with the given name, this is the case if a .stl, .obj file or a description from @@ -131,7 +134,7 @@ def check_with_extension(self, path: str) -> bool: full_path = pathlib.Path(os.path.join(self.cache_dir, file_name)) return full_path.exists() - def check_without_extension(self, path: str, object_description: 'ObjectDescription') -> bool: + def check_without_extension(self, path: str, object_description: ObjectDescription) -> bool: """ Check if the file in the given path exists in the cache directory the given file extension. Instead, replace the given extension with the extension of the used ObjectDescription and check for that one. diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 6dab0ebe4..0e0fe02e6 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -27,6 +27,22 @@ def get_point_as_list(point: Point) -> List[float]: return [point.x, point.y, point.z] +def get_list_from_points(points: List[Point]) -> List[List[float]]: + """ + :param points: The points as a list of Point instances. + :return: The points as a list of lists of floats + """ + return [get_point_as_list(point) for point in points] + + +def get_points_from_list(points: List[List[float]]) -> List[Point]: + """ + :param points: The points as a list of lists of floats. + :return: The points as a list of Point instances. + """ + return [Point(*point) for point in points] + + @dataclass class Color: """ @@ -100,58 +116,64 @@ class BoundingBox: max_y: float max_z: float - def get_points_list(self) -> List[List[float]]: + def get_axis_aligned_corners(self) -> List[List[float]]: """ :return: The points of the bounding box as a list of lists of floats. """ - return [[point.x, point.y, point.z] for point in self.get_points()] + return [[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]] - def get_points(self) -> List[Point]: + def get_transformed_min_max_points(self, transform: Transform) -> Tuple[Point, Point]: """ - :return: The points of the bounding box as a list of Point instances. + Apply a transformation to the bounding box and return the transformed minimum and maximum points. + + :param transform: The transformation to apply + :return: The transformed minimum and maximum points """ - return [Point(self.min_x, self.min_y, self.min_z), - Point(self.min_x, self.min_y, self.max_z), - Point(self.min_x, self.max_y, self.min_z), - Point(self.min_x, self.max_y, self.max_z), - Point(self.max_x, self.min_y, self.min_z), - Point(self.max_x, self.min_y, self.max_z), - Point(self.max_x, self.max_y, self.min_z), - Point(self.max_x, self.max_y, self.max_z)] + 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 Point(*min_p), Point(*max_p) def get_min_max_points(self) -> Tuple[Point, Point]: """ - :return: The axis-aligned bounding box as a tuple of minimum and maximum points + :return: The minimum and maximum points of the bounding box """ return self.get_min_point(), self.get_max_point() def get_min_point(self) -> Point: """ - :return: The axis-aligned bounding box as a minimum point + :return: The minimum point of the bounding box """ return Point(self.min_x, self.min_y, self.min_z) def get_max_point(self) -> Point: """ - :return: The axis-aligned bounding box as a maximum point + :return: The maximum point of the bounding box """ return Point(self.max_x, self.max_y, self.max_z) def get_min_max(self) -> Tuple[List[float], List[float]]: """ - :return: The axis-aligned bounding box as a tuple of minimum and maximum points + :return: The minimum and maximum points of the bounding box as lists of floats """ return self.get_min(), self.get_max() def get_min(self) -> List[float]: """ - :return: The minimum point of the axis-aligned bounding box + :return: The minimum point of the axis-aligned bounding box as a list of floats """ return [self.min_x, self.min_y, self.min_z] def get_max(self) -> List[float]: """ - :return: The maximum point of the axis-aligned bounding box + :return: The maximum point of the axis-aligned bounding box as a list of floats """ return [self.max_x, self.max_y, self.max_z] @@ -159,6 +181,12 @@ def get_max(self) -> List[float]: @dataclass class AxisAlignedBoundingBox(BoundingBox): + def get_corners(self) -> List[List[float]]: + """ + :return: The points of the axis-aligned bounding box. + """ + return self.get_axis_aligned_corners() + 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. @@ -193,10 +221,10 @@ def __init__(self, min_x: float, min_y: float, min_z: float, max_x: float, max_y self.min_x, self.min_y, self.min_z = min_x, min_y, min_z self.max_x, self.max_y, self.max_z = max_x, max_y, max_z self.transform: Transform = transform - self._points: Optional[List[Point]] = points + self._points_list: Optional[List[List[float]]] = get_list_from_points(points) if points is not None else None @classmethod - def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], transform: Transform): + def from_min_max_and_transform(cls, min_point: Sequence[float], max_point: Sequence[float], transform: Transform): """ Set the rotated bounding box from a minimum, maximum point, and a transformation. @@ -219,13 +247,7 @@ 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]: + def get_corners(self, transform: Optional[Transform] = None) -> List[List[float]]: """ :param transform: The transformation to apply to the points, if None the stored transformation is used. :return: The points of the rotated bounding box. @@ -233,17 +255,9 @@ def get_points(self, transform: Optional[Transform] = None) -> List[Point]: if (self._points is None) or (transform is not None): if transform is not None: self.transform = transform - points_array = np.array([[self.min_x, self.min_y, self.min_z], - [self.min_x, self.min_y, self.max_z], - [self.min_x, self.max_y, self.min_z], - [self.min_x, self.max_y, self.max_z], - [self.max_x, self.min_y, self.min_z], - [self.max_x, self.min_y, self.max_z], - [self.max_x, self.max_y, self.min_z], - [self.max_x, self.max_y, self.max_z]]) - transformed_points = self.transform.apply_transform_to_array_of_points(points_array).tolist() - self._points = [Point(*point) for point in transformed_points] - return self._points + points_array = np.array(self.get_axis_aligned_corners()) + self._points_list = self.transform.apply_transform_to_array_of_points(points_array).tolist() + return self._points_list @dataclass diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py index 014fba43e..a5e968a7a 100644 --- a/src/pycram/worlds/multiverse.py +++ b/src/pycram/worlds/multiverse.py @@ -8,7 +8,7 @@ from .multiverse_communication.client_manager import MultiverseClientManager from .multiverse_communication.clients import MultiverseController, MultiverseReader, MultiverseWriter, MultiverseAPI from ..config.multiverse_conf import MultiverseConfig -from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint, MeshVisualShape +from ..datastructures.dataclasses import AxisAlignedBoundingBox, Color, ContactPointsList, ContactPoint from ..datastructures.enums import WorldMode, JointType, ObjectType, MultiverseBodyProperty, MultiverseJointPosition, \ MultiverseJointCMD from ..datastructures.pose import Pose @@ -16,7 +16,7 @@ from ..description import Link, Joint from ..object_descriptors.mjcf import ObjectDescription as MJCF from ..robot_description import RobotDescription -from ..ros.logging import logwarn +from ..ros.logging import logwarn, logerr from ..utils import RayTestUtils, wxyz_to_xyzw, xyzw_to_wxyz from ..validation.goal_validator import validate_object_pose, validate_multiple_joint_positions, \ validate_joint_position, validate_multiple_object_poses @@ -327,7 +327,7 @@ def _set_multiple_joint_positions_using_controller(self, joint_positions: Dict[J :param joint_positions: The dictionary of joints and positions. """ controlled_joints_data = {self.get_actuator_for_joint(joint): - {self.get_joint_cmd_name(joint.type): [position]} + {self.get_joint_cmd_name(joint.type): [position]} for joint, position in joint_positions.items()} self.joint_controller.send_multiple_body_data_to_server(controlled_joints_data) return True @@ -621,30 +621,30 @@ def restore_physics_simulator_state(self, state_id: int) -> None: self.api_requester.load(self.saved_simulator_states[state_id]) def set_link_color(self, link: Link, rgba_color: Color): - rospy.logwarn("set_link_color is not implemented in Multiverse") + logwarn("set_link_color is not implemented in Multiverse") def get_link_color(self, link: Link) -> Color: - rospy.logwarn("get_link_color is not implemented in Multiverse") + logwarn("get_link_color is not implemented in Multiverse") return Color() def get_colors_of_object_links(self, obj: Object) -> Dict[str, Color]: - rospy.logwarn("get_colors_of_object_links is not implemented in Multiverse") + logwarn("get_colors_of_object_links is not implemented in Multiverse") return {} def get_object_axis_aligned_bounding_box(self, obj: Object) -> AxisAlignedBoundingBox: - rospy.logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse") + logerr("get_object_axis_aligned_bounding_box for multi-link objects is not implemented in Multiverse") raise NotImplementedError def get_link_axis_aligned_bounding_box(self, link: Link) -> AxisAlignedBoundingBox: - rospy.logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse") + logerr("get_link_axis_aligned_bounding_box is not implemented in Multiverse") raise NotImplementedError def set_realtime(self, real_time: bool) -> None: - rospy.logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the" - "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") + logwarn("set_realtime is not implemented as an API in Multiverse, it is configured in the" + "multiverse configuration file (.muv file) as rtf_required where a value of 1 means real-time") def set_gravity(self, gravity_vector: List[float]) -> None: - rospy.logwarn("set_gravity is not implemented in Multiverse") + logwarn("set_gravity is not implemented in Multiverse") def check_object_exists(self, obj: Object) -> bool: """ From cf019f2bb5d521dafb20a92a36d19616b4c2abba Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 11 Oct 2024 18:34:10 +0200 Subject: [PATCH 8/9] [MeshAndObject] corrected method names. --- src/pycram/datastructures/dataclasses.py | 10 +--------- src/pycram/description.py | 2 +- test/test_links.py | 4 ++-- 3 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 0e0fe02e6..e3e8490a9 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -35,14 +35,6 @@ def get_list_from_points(points: List[Point]) -> List[List[float]]: return [get_point_as_list(point) for point in points] -def get_points_from_list(points: List[List[float]]) -> List[Point]: - """ - :param points: The points as a list of lists of floats. - :return: The points as a list of Point instances. - """ - return [Point(*point) for point in points] - - @dataclass class Color: """ @@ -252,7 +244,7 @@ def get_corners(self, transform: Optional[Transform] = None) -> List[List[float] :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 (self._points_list is None) or (transform is not None): if transform is not None: self.transform = transform points_array = np.array(self.get_axis_aligned_corners()) diff --git a/src/pycram/description.py b/src/pycram/description.py index 244269a02..e0ec326b6 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -237,7 +237,7 @@ def get_bounding_box(self, rotated: bool = False) -> Union[AxisAlignedBoundingBo mesh = trimesh.load(mesh_path) min_bound, max_bound = mesh.bounds if rotated: - return RotatedBoundingBox.from_min_max(min_bound, max_bound, self.transform) + return RotatedBoundingBox.from_min_max_and_transform(min_bound, max_bound, self.transform) else: return AxisAlignedBoundingBox.from_min_max(min_bound, max_bound).get_transformed_box(self.transform) else: diff --git a/test/test_links.py b/test/test_links.py index 9cb01bbaa..8e35e8f27 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -12,9 +12,9 @@ class TestLinks(BulletWorldTestCase): def test_rotated_bounding_box(self): self.milk.set_pose(Pose([1, 1, 1], quaternion_from_euler(np.pi/4, 0, 0).tolist())) aabb = self.milk.get_axis_aligned_bounding_box() - aabb_points = np.array(aabb.get_points_list()) + aabb_points = np.array(aabb.get_corners()) rbb = self.milk.get_rotated_bounding_box() - rot_points = np.array(rbb.get_points_list()) + rot_points = np.array(rbb.get_corners()) plot = False if plot: fig = plt.figure() From d74f7d6dc1eb4b1983244077cf3aa89c7746bdc1 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Fri, 8 Nov 2024 09:06:24 +0100 Subject: [PATCH 9/9] [MeshAndObject] Added convex hull. --- src/pycram/datastructures/world.py | 19 ++++++++++++ src/pycram/description.py | 23 ++++++++++++++ src/pycram/world_concepts/world_object.py | 16 ++++++++-- test/test_links.py | 38 ++++++++++++++++------- 4 files changed, 83 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index a72b3629c..56351ff3a 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -9,6 +9,7 @@ import numpy as np from geometry_msgs.msg import Point +from trimesh.parent import Geometry3D from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type from ..cache_manager import CacheManager @@ -125,6 +126,24 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca self.on_add_object_callbacks: List[Callable[[Object], None]] = [] + def get_object_convex_hull(self, obj: Object) -> Geometry3D: + """ + Get the convex hull of an object. + + :param obj: The pycram object. + :return: The convex hull of the object as a list of Points. + """ + raise NotImplementedError + + def get_link_convex_hull(self, link: Link) -> Geometry3D: + """ + Get the convex hull of a link of an articulated object. + + :param link: The link as a AbstractLink object. + :return: The convex hull of the link as a list of Points. + """ + raise NotImplementedError + def add_callback_on_add_object(self, callback: Callable[[Object], None]) -> None: """ Add a callback that is called when an object is added to the world. diff --git a/src/pycram/description.py b/src/pycram/description.py index e0ec326b6..ffdf50853 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -5,6 +5,8 @@ import pathlib from abc import ABC, abstractmethod +from trimesh.parent import Geometry3D + from .ros.data_types import Time import trimesh from geometry_msgs.msg import Point, Quaternion @@ -247,6 +249,27 @@ def get_bounding_box(self, rotated: bool = False) -> Union[AxisAlignedBoundingBo else: return bounding_box.get_transformed_box(self.transform) + def get_convex_hull(self) -> Geometry3D: + """ + :return: The convex hull of the link geometry. + """ + try: + return self.world.get_link_convex_hull(self) + except NotImplementedError: + if isinstance(self.geometry, MeshVisualShape): + mesh_path = self.get_mesh_path() + mesh = trimesh.load(mesh_path) + return trimesh.convex.convex_hull(mesh).apply_transform(self.transform.get_homogeneous_matrix()) + else: + raise LinkGeometryHasNoMesh(self.name, type(self.geometry).__name__) + + def _plot_convex_hull(self): + """ + Plot the convex hull of the link geometry. + """ + hull = self.get_convex_hull() + hull.show() + def get_mesh_path(self) -> str: """ :return: The path of the mesh file of this link if the geometry is a mesh. diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index b01981d7d..fe9e4445b 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -7,6 +7,7 @@ import numpy as np from deprecated import deprecated from geometry_msgs.msg import Point, Quaternion +from trimesh.parent import Geometry3D from typing_extensions import Type, Optional, Dict, Tuple, List, Union from ..datastructures.dataclasses import (Color, ObjectState, LinkState, JointState, @@ -1356,7 +1357,7 @@ def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox: :return: The axis aligned bounding box of this object. """ if self.has_one_link: - return self.links[self.description.get_root()].get_bounding_box() + return self.root_link.get_bounding_box() else: return self.world.get_object_axis_aligned_bounding_box(self) @@ -1367,10 +1368,21 @@ def get_rotated_bounding_box(self) -> RotatedBoundingBox: :return: The rotated bounding box of this object. """ if self.has_one_link: - return self.links[self.description.get_root()].get_bounding_box(rotated=True) + return self.root_link.get_bounding_box(rotated=True) else: return self.world.get_object_rotated_bounding_box(self) + def get_convex_hull(self) -> Geometry3D: + """ + Return the convex hull of this object. + + :return: The convex hull of this object. + """ + if self.has_one_link: + return self.root_link.get_convex_hull() + else: + return self.world.get_object_convex_hull(self) + def get_base_origin(self) -> Pose: """ Return the origin of the base/bottom of this object. diff --git a/test/test_links.py b/test/test_links.py index 8e35e8f27..21370cf0d 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,6 +1,7 @@ import numpy as np from matplotlib import pyplot as plt from tf.transformations import quaternion_from_euler +from typing_extensions import List from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.dataclasses import Color @@ -9,6 +10,16 @@ class TestLinks(BulletWorldTestCase): + def test_get_convex_hull(self): + self.milk.set_orientation(quaternion_from_euler(0, np.pi/4, 0)) + hull = self.milk.root_link.get_convex_hull() + self.assertIsNotNone(hull) + self.assertTrue(len(hull.vertices) > 0) + self.assertTrue(len(hull.faces) > 0) + plot = False + if plot: + self.plot_3d_points([hull.vertices]) + def test_rotated_bounding_box(self): self.milk.set_pose(Pose([1, 1, 1], quaternion_from_euler(np.pi/4, 0, 0).tolist())) aabb = self.milk.get_axis_aligned_bounding_box() @@ -17,20 +28,25 @@ def test_rotated_bounding_box(self): rot_points = np.array(rbb.get_corners()) plot = False if plot: - fig = plt.figure() - ax = fig.add_subplot(projection='3d') + self.plot_3d_points([aabb_points, rot_points]) + + @staticmethod + def plot_3d_points(list_of_points: List[np.ndarray]): + fig = plt.figure() + ax = fig.add_subplot(projection='3d') - ax.scatter(rot_points[:, 0], rot_points[:, 1], rot_points[:, 2], c='r', marker='o') - ax.scatter(aabb_points[:, 0], aabb_points[:, 1], aabb_points[:, 2], c='b', marker='o') + for points in list_of_points: + color = np.random.rand(3,) + ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=color, marker='o') - ax.set_xlabel('X Label') - ax.set_ylabel('Y Label') - ax.set_zlabel('Z Label') - plt.xlim(0, 2) - plt.ylim(0, 2) - ax.set_zlim(0, 2) + ax.set_xlabel('X Label') + ax.set_ylabel('Y Label') + ax.set_zlabel('Z Label') + plt.xlim(0, 2) + plt.ylim(0, 2) + ax.set_zlim(0, 2) - plt.show() + plt.show() def test_add_constraint(self): milk_link = self.milk.root_link