From 3b4f7e5b5bfb2a65cfeefabf669e7c855ac8a1d0 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 16 Jan 2025 10:29:36 +0100 Subject: [PATCH 1/2] [world object] Fixed handling of scale_mesh and color params --- src/pycram/cache_manager.py | 7 +++++-- src/pycram/datastructures/world.py | 6 ++++-- src/pycram/description.py | 9 ++++++--- src/pycram/object_descriptors/generic.py | 2 +- src/pycram/object_descriptors/urdf.py | 22 +++++++++++++--------- src/pycram/world_concepts/world_object.py | 6 ++++-- test/test_butterworth_filter.py | 4 ++-- 7 files changed, 35 insertions(+), 21 deletions(-) diff --git a/src/pycram/cache_manager.py b/src/pycram/cache_manager.py index 29bc5c2ba..9a373d6fc 100644 --- a/src/pycram/cache_manager.py +++ b/src/pycram/cache_manager.py @@ -7,6 +7,7 @@ from typing_extensions import List, TYPE_CHECKING, Optional +from .datastructures.dataclasses import Color from .ros.logging import loginfo if TYPE_CHECKING: @@ -56,7 +57,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, - mesh_transform: Optional[Transform] = None) -> str: + mesh_transform: Optional[Transform] = None, + color: Optional[Color] = None) -> str: """ Check if the file is already in the cache directory, if not preprocess and save in the cache. @@ -67,6 +69,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, :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. + :param color: The color of the object. :return: The path of the cached file. """ path_object = pathlib.Path(path) @@ -82,7 +85,7 @@ def update_cache_dir_with_object(self, path: str, ignore_cached_files: bool, path = self.look_for_file_in_data_dir(path_object) object_description.original_path = path object_description.generate_description_from_file(path, object_name, extension, cache_path, - scale_mesh, mesh_transform) + scale_mesh, mesh_transform, color) return cache_path diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index ed5978cd0..8324b5e68 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -318,7 +318,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, - mesh_transform: Optional[Transform] = None) -> str: + mesh_transform: Optional[Transform] = None, + color: Optional[Color] = None) -> str: """ Update the cache directory with the given object. @@ -328,10 +329,11 @@ def preprocess_object_file_and_get_its_cache_path(self, path: str, ignore_cached :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. + :param color: The color of the object. :return: The path of the cached object. """ return self.cache_manager.update_cache_dir_with_object(path, ignore_cached_files, description, name, - scale_mesh, mesh_transform) + scale_mesh, mesh_transform, color) @property def simulation_time_step(self): diff --git a/src/pycram/description.py b/src/pycram/description.py index 10257a9ab..8f17ca8d2 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -830,7 +830,8 @@ def load_description(self, path: str) -> Any: def generate_description_from_file(self, path: str, name: str, extension: str, save_path: str, scale_mesh: Optional[float] = None, - mesh_transform: Optional[Transform] = None) -> None: + mesh_transform: Optional[Transform] = None, + color: Optional[Color] = 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. @@ -841,6 +842,7 @@ def generate_description_from_file(self, path: str, name: str, extension: str, s :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. + :param color: The color of the object. :raises ObjectDescriptionNotFound: If the description file could not be found/read. """ @@ -854,7 +856,7 @@ def generate_description_from_file(self, path: str, name: str, extension: str, s mesh.apply_transform(transform) path = path.replace(extension, ".obj") mesh.export(path) - self.generate_from_mesh_file(path, name, save_path=save_path) + self.generate_from_mesh_file(path, name, save_path=save_path, color=color, scale=scale_mesh) elif extension == self.get_file_extension(): self.generate_from_description_file(path, save_path=save_path) else: @@ -910,7 +912,7 @@ def get_file_name(self, path_object: pathlib.Path, extension: str, object_name: @classmethod @abstractmethod - def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> None: + def generate_from_mesh_file(cls, path: str, name: str, save_path: str, color: Color) -> None: """ Generate a description file from one of the mesh types defined in the mesh_extensions and return the path of the generated file. The generated file will be saved at the given save_path. @@ -918,6 +920,7 @@ def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> None: :param path: The path to the .obj file. :param name: The name of the object. :param save_path: The path to save the generated description file. + :param color: The color of the object. """ pass diff --git a/src/pycram/object_descriptors/generic.py b/src/pycram/object_descriptors/generic.py index fb2b456ec..d8b8ed4f1 100644 --- a/src/pycram/object_descriptors/generic.py +++ b/src/pycram/object_descriptors/generic.py @@ -113,7 +113,7 @@ def load_description(self, path: str) -> Any: ... @classmethod - def generate_from_mesh_file(cls, path: str, name: str, save_path: str) -> str: + def generate_from_mesh_file(cls, path: str, name: str, save_path: str, color: Color) -> str: raise NotImplementedError @classmethod diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py index bdb48b8f7..8f36c90a1 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -255,7 +255,7 @@ def load_description(self, path) -> URDF: with suppress_stdout_stderr(): return URDF.from_xml_string(file.read()) - def generate_from_mesh_file(self, path: str, name: str, save_path: str, color: Optional[Color] = Color()) -> None: + def generate_from_mesh_file(self, path: str, name: str, save_path: str, color: Optional[Color] = Color(), scale = Optional[float]) -> None: """ Generate a URDf file with the given .obj or .stl file as mesh. In addition, use the given rgba_color to create a material tag in the URDF. The URDF file will be saved to the given save_path. @@ -264,30 +264,34 @@ def generate_from_mesh_file(self, path: str, name: str, save_path: str, color: O :param name: The name of the object. :param save_path: The path to save the URDF file to. :param color: The color of the object. + :param scale: The scale of the mesh. """ urdf_template = ' \n \ \n \ + \n \ + \n \ + \n \ \n \ \n \ \n \ - \n \ + \n \ \n \ - \n \ - \n \ - \n \ + \n \ \n \ \n \ - \n \ - \n \ - \n \ + \n \ + \n \ + \n \ + \n \ \n \ \n \ ' urdf_template = self.fix_missing_inertial(urdf_template) rgb = " ".join(list(map(str, color.get_rgba()))) + s = " ".join([str(scale)] * 3) pathlib_obj = pathlib.Path(path) path = str(pathlib_obj.resolve()) - content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb) + content = urdf_template.replace("~a", name).replace("~b", path).replace("~c", rgb).replace("~d", s) self.write_description_to_file(content, save_path) def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 034385126..71c89e1e5 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -102,12 +102,14 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] self.local_transformer = LocalTransformer() self.original_pose = self.local_transformer.transform_pose(pose, "map") self._current_pose = self.original_pose + self.scale_mesh = scale_mesh if scale_mesh is not None else 1.0 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, - mesh_transform=mesh_transform) + scale_mesh=self.scale_mesh, + mesh_transform=mesh_transform, + color=self.color) self.description.update_description_from_file(self.path) diff --git a/test/test_butterworth_filter.py b/test/test_butterworth_filter.py index bcbf9f95f..efd30338d 100644 --- a/test/test_butterworth_filter.py +++ b/test/test_butterworth_filter.py @@ -37,8 +37,8 @@ def test_filter_single_value_data(self): filter = Butterworth() data = [1] filtered_data = filter.filter(data) - expected_filtered_data = [0.026077721701092293] # The expected filtered value - self.assertEqual(filtered_data.tolist(), expected_filtered_data) + expected_filtered_data = 0.026077721701092293 # The expected filtered value + self.assertAlmostEquals(filtered_data.tolist()[0], expected_filtered_data) if __name__ == '__main__': unittest.main() \ No newline at end of file From 362ced965417813c922dcc0843ae500190510e22 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 16 Jan 2025 13:14:46 +0100 Subject: [PATCH 2/2] [action desigs] ParkArms and MoveGripper handle Arms.BOTH correctly --- src/pycram/designators/action_designator.py | 34 +++++++++------------ 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 1ca3464b1..1e09ab1d7 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -165,7 +165,12 @@ class SetGripperActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() + arm_chains = RobotDescription.current_robot_description.get_arm_chain(self.gripper) + if type(arm_chains) is not list: + MoveGripperMotion(gripper=arm_chains.arm_type, motion=self.motion).perform() + else: + for chain in arm_chains: + MoveGripperMotion(gripper=chain.arm_type, motion=self.motion).perform() @dataclass @@ -215,24 +220,15 @@ class ParkArmsActionPerformable(ActionAbstract): @with_tree def plan(self) -> None: - # create the keyword arguments - kwargs = dict() - left_poses = None - right_poses = None - - # add park left arm if wanted - if self.arm in [Arms.LEFT, Arms.BOTH]: - kwargs["left_arm_config"] = "park" - left_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT).get_static_joint_states( - kwargs["left_arm_config"]) - - # add park right arm if wanted - if self.arm in [Arms.RIGHT, Arms.BOTH]: - kwargs["right_arm_config"] = "park" - right_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT).get_static_joint_states( - kwargs["right_arm_config"]) - - MoveArmJointsMotion(left_poses, right_poses).perform() + joint_poses = {} + arm_chains = RobotDescription.current_robot_description.get_arm_chain(self.arm) + if type(arm_chains) is not list: + joint_poses = arm_chains.get_static_joint_states("park") + else: + for arm_chain in RobotDescription.current_robot_description.get_arm_chain(self.arm): + joint_poses.update(arm_chain.get_static_joint_states("park")) + + MoveJointsMotion(names=list(joint_poses.keys()), positions=list(joint_poses.values())).perform() @dataclass