Merge pull request #257 from Tigul/arm-fixes

Arm fixes

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
 

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

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,14 +857,15 @@ 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.
 
         :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
 

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

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

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 = '<?xml version="0.0" ?> \n \
                         <robot name="~a_object"> \n \
+                        <material name="color">\n \
+                                <color rgba="~c"/>\n \
+                        </material>\n \
                          <link name="~a_main"> \n \
                             <visual> \n \
                                 <geometry>\n \
-                                    <mesh filename="~b" scale="1 1 1"/> \n \
+                                    <mesh filename="~b" scale="~d"/> \n \
                                 </geometry>\n \
-                                <material name="white">\n \
-                                    <rgba_color rgba="~c"/>\n \
-                                </material>\n \
+                                <material name="color"/>\n \
                           </visual> \n \
                         <collision> \n \
-                        <geometry>\n \
-                            <mesh filename="~b" scale="1 1 1"/>\n \
-                        </geometry>\n \
+                            <geometry>\n \
+                                <mesh filename="~b" scale="~d"/>\n \
+                            </geometry>\n \
+                            <material name="color"/>\n \
                         </collision>\n \
                         </link> \n \
                         </robot>'
         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:

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)
 

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()