Feature: Manipulator Samples and Poses (#163)

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

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>

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>

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>

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>

clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro

@@ -1,18 +1,31 @@
 <?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"/>
         <joint name="${name}_right_finger_tip_joint"/>
         <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>

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"/>

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>