From db4ea251e7196d78d9e83a1a2a22aff0c334188f Mon Sep 17 00:00:00 2001
From: luis-camero <88782189+luis-camero@users.noreply.github.com>
Date: Tue, 11 Feb 2025 10:29:24 -0500
Subject: [PATCH] Feature: Manipulator Samples and Poses (#163)

* Add group_state macros to arm SRDF

* Add group_state macros to grippers SRDF

* Generate pose macros in URDF
---
 .../semantic_description/generator.py         | 17 +++++++
 .../semantic_description/manipulators.py      | 26 +++++++++-
 .../srdf/arm/kinova_gen3_6dof.srdf.xacro      | 41 ++++++++++------
 .../srdf/arm/kinova_gen3_7dof.srdf.xacro      | 44 ++++++++++-------
 .../srdf/arm/kinova_gen3_lite.srdf.xacro      | 26 ++++++++++
 .../srdf/arm/universal_robots.srdf.xacro      | 49 ++++++++++---------
 .../srdf/gripper/kinova_2f_lite.srdf.xacro    | 25 +++++++---
 .../srdf/gripper/robotiq_2f_140.srdf.xacro    | 25 +++++++---
 .../srdf/gripper/robotiq_2f_85.srdf.xacro     | 26 +++++++---
 9 files changed, 201 insertions(+), 78 deletions(-)

diff --git a/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py b/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py
index 653a809b..8de0832b 100644
--- a/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py
+++ b/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,
diff --git a/clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py b/clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py
index 8bac0ba3..1571b9d7 100644
--- a/clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py
+++ b/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():
diff --git a/clearpath_manipulators_description/srdf/arm/kinova_gen3_6dof.srdf.xacro b/clearpath_manipulators_description/srdf/arm/kinova_gen3_6dof.srdf.xacro
index efc0be6c..535198be 100644
--- a/clearpath_manipulators_description/srdf/arm/kinova_gen3_6dof.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/arm/kinova_gen3_6dof.srdf.xacro
@@ -1,5 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="kinova_gen3_6dof_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_joint_1" value="${joint_positions[0]}"/>
+      <joint name="${name}_joint_2" value="${joint_positions[1]}"/>
+      <joint name="${name}_joint_3" value="${joint_positions[2]}"/>
+      <joint name="${name}_joint_4" value="${joint_positions[3]}"/>
+      <joint name="${name}_joint_5" value="${joint_positions[4]}"/>
+      <joint name="${name}_joint_6" value="${joint_positions[5]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="kinova_gen3_6dof" params="name">
     <group name="${name}">
         <joint name="${name}_joint_1"/>
@@ -9,21 +21,18 @@
         <joint name="${name}_joint_5"/>
         <joint name="${name}_joint_6"/>
     </group>
-    <group_state name="home" group="${name}">
-        <joint name="${name}_joint_1" value="0.0174"/>
-        <joint name="${name}_joint_2" value="-0.5725"/>
-        <joint name="${name}_joint_3" value="-0.9846"/>
-        <joint name="${name}_joint_4" value="0.1562"/>
-        <joint name="${name}_joint_5" value="-1.0719"/>
-        <joint name="${name}_joint_6" value="-1.7183"/>
-    </group_state>
-    <group_state name="zero" group="${name}">
-        <joint name="${name}_joint_1" value="0"/>
-        <joint name="${name}_joint_2" value="0"/>
-        <joint name="${name}_joint_3" value="0"/>
-        <joint name="${name}_joint_4" value="0"/>
-        <joint name="${name}_joint_5" value="0"/>
-        <joint name="${name}_joint_6" value="0"/>
-    </group_state>
+
+    <xacro:kinova_gen3_6dof_group_state
+      name="${name}"
+      group_state="home"
+      joint_positions="${[0.0174, -0.5725, -0.9846, 0.1562, -1.0719, -1.7183]}"
+    />
+
+    <xacro:kinova_gen3_6dof_group_state
+      name="${name}"
+      group_state="zero"
+      joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/clearpath_manipulators_description/srdf/arm/kinova_gen3_7dof.srdf.xacro b/clearpath_manipulators_description/srdf/arm/kinova_gen3_7dof.srdf.xacro
index c52ee0a3..6285c2b3 100644
--- a/clearpath_manipulators_description/srdf/arm/kinova_gen3_7dof.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/arm/kinova_gen3_7dof.srdf.xacro
@@ -1,5 +1,18 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="kinova_gen3_7dof_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_joint_1" value="${joint_positions[0]}"/>
+      <joint name="${name}_joint_2" value="${joint_positions[1]}"/>
+      <joint name="${name}_joint_3" value="${joint_positions[2]}"/>
+      <joint name="${name}_joint_4" value="${joint_positions[3]}"/>
+      <joint name="${name}_joint_5" value="${joint_positions[4]}"/>
+      <joint name="${name}_joint_6" value="${joint_positions[5]}"/>
+      <joint name="${name}_joint_7" value="${joint_positions[6]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="kinova_gen3_7dof" params="name">
     <group name="${name}">
         <joint name="${name}_joint_1"/>
@@ -10,23 +23,18 @@
         <joint name="${name}_joint_6"/>
         <joint name="${name}_joint_7"/>
     </group>
-    <group_state name="home" group="${name}">
-        <joint name="${name}_joint_1" value="0.0174"/>
-        <joint name="${name}_joint_2" value="-0.5725"/>
-        <joint name="${name}_joint_3" value="-0.9846"/>
-        <joint name="${name}_joint_4" value="0.1562"/>
-        <joint name="${name}_joint_5" value="-1.0719"/>
-        <joint name="${name}_joint_6" value="-1.7183"/>
-        <joint name="${name}_joint_7" value="-1.7183"/>
-    </group_state>
-    <group_state name="zero" group="${name}">
-        <joint name="${name}_joint_1" value="0"/>
-        <joint name="${name}_joint_2" value="0"/>
-        <joint name="${name}_joint_3" value="0"/>
-        <joint name="${name}_joint_4" value="0"/>
-        <joint name="${name}_joint_5" value="0"/>
-        <joint name="${name}_joint_6" value="0"/>
-        <joint name="${name}_joint_7" value="0"/>
-    </group_state>
+
+    <xacro:kinova_gen3_7dof_group_state
+      name="${name}"
+      group_state="home"
+      joint_positions="${[0.0174, -0.5725, -0.9846, 0.1562, -1.0719, -1.7183, -1.7183]}"
+    />
+
+    <xacro:kinova_gen3_7dof_group_state
+      name="${name}"
+      group_state="zero"
+      joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/clearpath_manipulators_description/srdf/arm/kinova_gen3_lite.srdf.xacro b/clearpath_manipulators_description/srdf/arm/kinova_gen3_lite.srdf.xacro
index 80498a03..38f512de 100644
--- a/clearpath_manipulators_description/srdf/arm/kinova_gen3_lite.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/arm/kinova_gen3_lite.srdf.xacro
@@ -1,5 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="kinova_gen3_lite_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_joint_1" value="${joint_positions[0]}"/>
+      <joint name="${name}_joint_2" value="${joint_positions[1]}"/>
+      <joint name="${name}_joint_3" value="${joint_positions[2]}"/>
+      <joint name="${name}_joint_4" value="${joint_positions[3]}"/>
+      <joint name="${name}_joint_5" value="${joint_positions[4]}"/>
+      <joint name="${name}_joint_6" value="${joint_positions[5]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="kinova_gen3_lite" params="name">
     <group name="${name}">
         <joint name="${name}_joint_1"/>
@@ -33,5 +45,19 @@
         <joint name="${name}_joint_5" value="0"/>
         <joint name="${name}_joint_6" value="0"/>
     </group_state>
+
+
+    <xacro:kinova_gen3_lite_group_state
+      name="${name}"
+      group_state="home"
+      joint_positions="${[0.0, 0.0, math.pi * 1/2, math.pi * 1/2, math.pi * 1/2, math.pi * -1/2]}"
+    />
+
+    <xacro:kinova_gen3_lite_group_state
+      name="${name}"
+      group_state="zero"
+      joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/clearpath_manipulators_description/srdf/arm/universal_robots.srdf.xacro b/clearpath_manipulators_description/srdf/arm/universal_robots.srdf.xacro
index 65202af1..5c942a8a 100644
--- a/clearpath_manipulators_description/srdf/arm/universal_robots.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/arm/universal_robots.srdf.xacro
@@ -1,5 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="universal_robots_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_shoulder_pan_joint"  value="${joint_positions[0]}"/>
+      <joint name="${name}_shoulder_lift_joint" value="${joint_positions[1]}"/>
+      <joint name="${name}_elbow_joint"         value="${joint_positions[2]}"/>
+      <joint name="${name}_wrist_1_joint"       value="${joint_positions[3]}"/>
+      <joint name="${name}_wrist_2_joint"       value="${joint_positions[4]}"/>
+      <joint name="${name}_wrist_3_joint"       value="${joint_positions[5]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="universal_robots" params="name">
     <group name="${name}">
         <joint name="${name}_shoulder_pan_joint"/>
@@ -9,29 +21,18 @@
         <joint name="${name}_wrist_2_joint"/>
         <joint name="${name}_wrist_3_joint"/>
     </group>
-    <group_state name="home" group="${name}">
-        <joint name="${name}_shoulder_pan_joint" value="0.0"/>
-        <joint name="${name}_shoulder_lift_joint" value="-1.5707"/>
-        <joint name="${name}_elbow_joint" value="1.5707"/>
-        <joint name="${name}_wrist_1_joint" value="-1.5707"/>
-        <joint name="${name}_wrist_2_joint" value="-1.5707"/>
-        <joint name="${name}_wrist_3_joint" value="0.0"/>
-      </group_state>
-    <group_state name="stow" group="${name}">
-      <joint name="${name}_shoulder_pan_joint" value="0.0"/>
-      <joint name="${name}_shoulder_lift_joint" value="-2.356"/>
-      <joint name="${name}_elbow_joint" value="2.356"/>
-      <joint name="${name}_wrist_1_joint" value="-1.5707"/>
-      <joint name="${name}_wrist_2_joint" value="-1.5707"/>
-      <joint name="${name}_wrist_3_joint" value="0.0"/>
-    </group_state>
-    <group_state name="zero" group="${name}">
-        <joint name="${name}_shoulder_pan_joint" value="0"/>
-        <joint name="${name}_shoulder_lift_joint" value="0"/>
-        <joint name="${name}_elbow_joint" value="0"/>
-        <joint name="${name}_wrist_1_joint" value="0"/>
-        <joint name="${name}_wrist_2_joint" value="0"/>
-        <joint name="${name}_wrist_3_joint" value="0"/>
-    </group_state>
+
+    <xacro:universal_robots_group_state
+      name="${name}"
+      group_state="home"
+      joint_positions="${[0.0, -1.5707, 1.5707, -1.5707, -1.5707, 0.0]}"
+    />
+
+    <xacro:universal_robots_group_state
+      name="${name}"
+      group_state="zero"
+      joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro b/clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro
index 6493a9cd..ae49f66f 100644
--- a/clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro
@@ -1,5 +1,12 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="kinova_2f_lite_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_right_finger_bottom_joint" value="${joint_positions[0]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="kinova_2f_lite" params="name">
     <group name="${name}">
         <joint name="${name}_right_finger_bottom_joint"/>
@@ -7,12 +14,18 @@
         <joint name="${name}_left_finger_bottom_joint"/>
         <joint name="${name}_left_finger_tip_joint"/>
     </group>
-    <group_state name="open" group="${name}">
-        <joint name="${name}_right_finger_bottom_joint" value="0.0"/>
-    </group_state>
-    <group_state name="close" group="${name}">
-        <joint name="${name}_right_finger_bottom_joint" value="0.85"/>
-    </group_state>
+
+    <xacro:kinova_2f_lite_group_state
+      name="${name}"
+      group_state="open"
+      joint_positions="${[0.0]}"
+    />
+
+    <xacro:kinova_2f_lite_group_state
+      name="${name}"
+      group_state="close"
+      joint_positions="${[0.85]}"
+    />
 
     <disable_collisions link1="${name}_right_finger_dist_link" link2="${name}_left_finger_dist_link" reason="User"/>
   </xacro:macro>
diff --git a/clearpath_manipulators_description/srdf/gripper/robotiq_2f_140.srdf.xacro b/clearpath_manipulators_description/srdf/gripper/robotiq_2f_140.srdf.xacro
index 1eb0ab9e..167d747b 100644
--- a/clearpath_manipulators_description/srdf/gripper/robotiq_2f_140.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/gripper/robotiq_2f_140.srdf.xacro
@@ -1,5 +1,12 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="robotiq_2f_140_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_finger_joint" value="${joint_positions[0]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="robotiq_2f_140" params="name">
     <group name="${name}">
         <joint name="${name}_finger_joint"/>
@@ -9,12 +16,18 @@
         <joint name="${name}_right_inner_knuckle_joint"/>
         <joint name="${name}_right_inner_finger_joint"/>
     </group>
-    <group_state name="open" group="${name}">
-        <joint name="${name}_finger_joint" value="0.0"/>
-    </group_state>
-    <group_state name="close" group="${name}">
-        <joint name="${name}_finger_joint" value="0.698"/>
-    </group_state>
+
+    <xacro:robotiq_2f_140_group_state
+      name="${name}"
+      group_state="open"
+      joint_positions="${[0.0]}"
+    />
+
+    <xacro:robotiq_2f_140_group_state
+      name="${name}"
+      group_state="close"
+      joint_positions="${[0.7]}"
+    />
 
     <disable_collisions link1="${name}_left_inner_knuckle" link2="${name}_left_inner_finger" reason="User"/>
     <disable_collisions link1="${name}_right_inner_knuckle" link2="${name}_right_inner_finger" reason="User"/>
diff --git a/clearpath_manipulators_description/srdf/gripper/robotiq_2f_85.srdf.xacro b/clearpath_manipulators_description/srdf/gripper/robotiq_2f_85.srdf.xacro
index 353d15c4..9b6d0983 100644
--- a/clearpath_manipulators_description/srdf/gripper/robotiq_2f_85.srdf.xacro
+++ b/clearpath_manipulators_description/srdf/gripper/robotiq_2f_85.srdf.xacro
@@ -1,5 +1,12 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="robotiq_2f_85_group_state" params="name group_state joint_positions">
+    <group_state name="${group_state}" group="${name}">
+      <joint name="${name}_robotiq_85_left_knuckle_joint" value="${joint_positions[0]}"/>
+    </group_state>
+  </xacro:macro>
+
   <xacro:macro name="robotiq_2f_85" params="name">
     <group name="${name}">
         <joint name="${name}_robotiq_85_base_joint"/>
@@ -12,16 +19,21 @@
         <joint name="${name}_robotiq_85_right_finger_joint"/>
         <joint name="${name}_robotiq_85_right_finger_tip_joint"/>
     </group>
-    <group_state name="open" group="${name}">
-        <joint name="${name}_robotiq_85_left_knuckle_joint" value="0.0"/>
-    </group_state>
-    <group_state name="close" group="${name}">
-        <joint name="${name}_robotiq_85_left_knuckle_joint" value="0.8"/>
-    </group_state>
+
+    <xacro:robotiq_2f_85_group_state
+      name="${name}"
+      group_state="open"
+      joint_positions="${[0.0]}"
+    />
+
+    <xacro:robotiq_2f_85_group_state
+      name="${name}"
+      group_state="close"
+      joint_positions="${[0.8]}"
+    />
 
     <disable_collisions link1="${name}_robotiq_85_left_inner_knuckle_link" link2="${name}_robotiq_85_left_finger_tip_link" reason="User"/>
     <disable_collisions link1="${name}_robotiq_85_right_inner_knuckle_link" link2="${name}_robotiq_85_right_finger_tip_link" reason="User"/>
-
   </xacro:macro>
 
 </robot>