Pass poses from config with arm_id on Franka

Author: luis-camero

clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py

@@ -79,9 +79,9 @@ def generate_arms(self) -> None:
             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(),
+                    macro=pose_macro.macro,
+                    parameters=pose_macro.parameters,
+                    blocks=pose_macro.blocks,
                 )
 
             self.xacro_writer.write_macro(
@@ -113,9 +113,9 @@ def generate_grippers(self) -> None:
             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(),
+                    macro=pose_macro.macro,
+                    parameters=pose_macro.parameters,
+                    blocks=pose_macro.blocks,
                 )
 
             self.xacro_writer.write_macro(

clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py

@@ -39,23 +39,42 @@
 
 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 BasePoseMacro():
+        NAME = 'name'
+        GROUP_STATE = 'group_state'
+        JOINT_POSITIONS = 'joint_positions'
+
+        def __init__(self, manipulator: BaseManipulator, pose: ManipulatorPose) -> None:
+            self.manipulator = manipulator
+            self.pose = pose
+            # Extract Joint Values
+            joints_string = [f'{joint:.4f}' for joint in self.pose.joints]
+            # Macro
+            self.macro = f'{self.manipulator.MANIPULATOR_MODEL}_group_state'
+            # Parameters
+            self.parameters = {
+                self.NAME: self.manipulator.name,
+                self.GROUP_STATE: self.pose.name,
+                self.JOINT_POSITIONS: f'${{[{", ".join(joints_string)}]}}'
+            }
+            # Blocks
+            self.blocks = None
+
+    class FrankaPoseMacro(BasePoseMacro):
+
+        def __init__(self, manipulator, pose):
+            super().__init__(manipulator, pose)
+            self.parameters[Franka.ARM_ID] = self.manipulator.arm_id
+
+    MODEL = {
+        Franka.MANIPULATOR_MODEL: FrankaPoseMacro,
+        FrankaGripper.MANIPULATOR_MODEL: FrankaPoseMacro
+    }
+
+    def __new__(cls, manipulator: BaseManipulator, pose: ManipulatorPose) -> BaseManipulator:
+        return ManipulatorPoseMacro.MODEL.setdefault(
+            manipulator.MANIPULATOR_MODEL,
+            ManipulatorPoseMacro.BasePoseMacro)(manipulator, pose)
 
 
 class ManipulatorSemanticDescription():