Generate pose macros in URDF

Author: luis-camero

clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py

@@ -32,6 +32,7 @@
 from clearpath_generator_common.common import BaseGenerator
 from clearpath_generator_common.description.writer import XacroWriter
 from clearpath_generator_common.semantic_description.manipulators import (
+    ManipulatorPoseMacro,
     ManipulatorSemanticDescription
 )
 
@@ -75,6 +76,14 @@ def generate_arms(self) -> None:
                 path=arm_semantic_description.path,
             )
 
+            for pose in arm.poses:
+                pose_macro = ManipulatorPoseMacro(arm, pose)
+                self.xacro_writer.write_macro(
+                    macro=pose_macro.macro(),
+                    parameters=pose_macro.parameters(),
+                    blocks=pose_macro.blocks(),
+                )
+
             self.xacro_writer.write_macro(
                 macro='{0}'.format(arm_semantic_description.model),
                 parameters=arm_semantic_description.parameters,
@@ -101,6 +110,14 @@ def generate_grippers(self) -> None:
                 path=gripper_semantic_description.path,
             )
 
+            for pose in arm.gripper.poses:
+                pose_macro = ManipulatorPoseMacro(arm.gripper, pose)
+                self.xacro_writer.write_macro(
+                    macro=pose_macro.macro(),
+                    parameters=pose_macro.parameters(),
+                    blocks=pose_macro.blocks(),
+                )
+
             self.xacro_writer.write_macro(
                 macro='{0}'.format(gripper_semantic_description.model),
                 parameters=gripper_semantic_description.parameters,

clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py

@@ -29,7 +29,31 @@
 # Redistribution and use in source and binary forms, with or without
 # modification, is not permitted without the express permission
 # of Clearpath Robotics.
-from clearpath_config.manipulators.types.manipulator import BaseManipulator
+from clearpath_config.manipulators.types.manipulator import (
+  BaseManipulator,
+  ManipulatorPose
+)
+
+
+class ManipulatorPoseMacro():
+
+    def __init__(self, manipulator: BaseManipulator, pose: ManipulatorPose) -> None:
+        self.manipulator = manipulator
+        self.pose = pose
+
+    def macro(self) -> str:
+        return f"{self.manipulator.MANIPULATOR_MODEL}_group_state"
+
+    def parameters(self) -> dict:
+        str_joints = [f'{joint:.4f}' for joint in self.pose.joints]
+        return {
+            'name': self.manipulator.name,
+            'group_state': self.pose.name,
+            'joint_positions': f"{{[{', '.join(str_joints)}]}}"
+        }
+
+    def blocks(self) -> str:
+        return None
 
 
 class ManipulatorSemanticDescription():