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 13d2a9a9e..68a3e8385 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -289,7 +289,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. @@ -299,10 +300,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 ff14c1f91..614f688bb 100644 --- a/src/pycram/description.py +++ b/src/pycram/description.py @@ -775,7 +775,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. @@ -786,6 +787,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. """ @@ -799,7 +801,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: @@ -855,7 +857,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. @@ -863,6 +865,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/designators/action_designator.py b/src/pycram/designators/action_designator.py index 9a502f34e..6f7ea4539 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -166,7 +166,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 @@ -216,24 +221,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 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 bf7c196a8..5347f8474 100644 --- a/src/pycram/object_descriptors/urdf.py +++ b/src/pycram/object_descriptors/urdf.py @@ -259,7 +259,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. @@ -268,30 +268,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 7225fb84b..3cfe90ac3 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -101,12 +101,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