From e2e4c97166c51abc2033f4480f6c73fafb3098bb Mon Sep 17 00:00:00 2001
From: Ethan Fowler <ethan@shadowrobot.com>
Date: Tue, 4 Aug 2020 14:43:08 +0000
Subject: [PATCH 1/5] WIP

---
 .../sr_bimanual_hardware_control_loop.launch  | 51 +++++++++++--------
 .../launch/sr_ur_arms_hands.launch            | 45 +++++++++-------
 2 files changed, 58 insertions(+), 38 deletions(-)

diff --git a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
index a60dfd821..d5344bb96 100644
--- a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
@@ -2,20 +2,26 @@
   <arg name="debug" default="false"/>
   <arg name="robot_state_pub_frequency" default="250"/>
   <arg name="joint_state_pub_frequency" default="125"/>
-  <!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
-  <arg name="hands" default="both"/>
-  <arg name="arm_ctrl" default="true"/>	
+
   <!-- ROBOT CONFIG-->
   <arg name="robot_model" default="ur10"/>
+  <!-- Whether there are arms. -->
+  <arg name="arms" default="true"/>
+  <!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
+  <arg name="hands" default="both"/>
+  <!-- Whether to run arm controllers. -->
+  <arg name="arm_ctrl" default="$(arg arms)"/>
+  <!-- Whether to run hand controllers. -->
+  <arg name="hand_ctrl" default="$(eval hands != 'none')"/>
   <!-- HANDS AND ARMS -->
-  <arg name="robot_description" if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
-  <arg name="robot_config_file" if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
+  <arg name="robot_description" if="$(eval arg('hands') != 'none' and arg('arms'))" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
+  <arg name="robot_config_file" if="$(eval arg('hands') != 'none' and arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
   <!-- ARMS BUT NO HANDS -->
-  <arg name="robot_description" if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))" default="'$(find sr_multi_description)/urdf/bimanual_$(arg robot_model).urdf.xacro'"/>
-  <arg name="robot_config_file" if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_$(arg robot_model).yaml"/>
+  <arg name="robot_description" if="$(eval arg('hands') == 'none' and arg('arms'))" default="'$(find sr_multi_description)/urdf/bimanual_$(arg robot_model).urdf.xacro'"/>
+  <arg name="robot_config_file" if="$(eval arg('hands') == 'none' and arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_$(arg robot_model).yaml"/>
   <!-- HANDS BUT NO ARMS -->
-  <arg name="robot_description" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))" default="'$(find sr_description)/robots/bimanual_shadowhand_motor_plus.urdf.xacro'"/>
-  <arg name="robot_config_file" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh.yaml"/>
+  <arg name="robot_description" if="$(eval arg('hands') != 'none' and not arg('arms'))" default="'$(find sr_description)/robots/bimanual_shadowhand_motor_plus.urdf.xacro'"/>
+  <arg name="robot_config_file" if="$(eval arg('hands') != 'none' and not arg('arms'))" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh.yaml"/>
 
 
   <!-- setting this parameter to false allows to load the robot_description from a higher level -->
@@ -31,14 +37,15 @@
   <arg name="eth_port" default="$(optenv ETHERCAT_PORT eth0)"/>
   <!-- The control mode PWM (true) or torque (false) -->
   <arg name="pwm_control" default="true"/>
-  <arg name="hand_trajectory" unless="$(eval hands == 'none')" default="true"/>
-  <arg name="hand_trajectory" if="$(eval hands == 'none')" default="false"/>
+  <!-- Set to true if you want to use grasp controller -->
+  <arg name="grasp_controller" default="false"/>
+  <arg name="hand_trajectory" default="true"/>
 
   <!-- ARMS CONFIG-->
   <arg name="arm_1_z" default="0.7551"/>
   <arg name="arm_2_z" default="0.7551"/>
-  <arg name="arm_x_separation" default="-0.4" if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))"/>
-  <arg name="arm_x_separation" default="0.0" unless="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))"/>
+  <arg name="arm_x_separation" default="-0.4" if="$(eval not arg('hands') == 'none' and not arg('arms'))"/>
+  <arg name="arm_x_separation" default="0.0" unless="$(eval not arg('hands') == 'none' and not arg('arms'))"/>
   <arg name="arm_y_separation" default="1.5"/>
   <arg name="arm_robot_hw_1" default="$(find sr_robot_launch)/config/right_ur_arm_robot_hw.yaml"/>
   <arg name="arm_robot_hw_2" default="$(find sr_robot_launch)/config/left_bimanual_ur_arm_robot_hw.yaml"/>
@@ -75,8 +82,13 @@
 
   <include file="$(find sr_robot_launch)/launch/bimanual_controller_stopper.launch"/>
 
+  <!-- Default hand controller groups -->
+  <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
+  <arg if="$(eval hand_trajectory and not grasp_controller)" name="hand_controller_group" default="trajectory"/>
+  <arg if="$(eval not hand_trajectory and not grasp_controller)" name="hand_controller_group" default="position"/>
+
   <!-- Controller -->
-  <group unless="$(eval hands == 'none')">
+  <group if="$(arg hand_ctrl)">
     <!-- Launch rosparam for payload if we use hand. If we only use hand and no arm the extra values wont be used-->
     <rosparam file="$(arg robot_config_file)"/>
     <!-- HAND (N.B. Arm robot harware is implicitly started here if ra_sr_ur_robot_hw is present in param /robot_hardware-->
@@ -99,15 +111,14 @@
       <arg name="load_robot_description" value="$(arg load_robot_description)"/>
     </include>
     <node name="bimanual_trajectory_controller" pkg="sr_utilities" type="controller_spawner.py" output="screen">
-      <param if="$(arg hand_trajectory)" name="controller_group" value="trajectory"/>
-      <param unless="$(arg hand_trajectory)" name="controller_group" value="position"/>
+      <param name="controller_group" value="$(arg hand_controller_group)"/>
       <param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
       <param name="wait_for" value="calibrated"/>
     </node>
   </group>
 
   <!-- Set rosparam if both hands and arms are being used -->
-  <group if="$(eval not arg('hands') == 'none' and arg('arm_ctrl'))">
+  <group if="$(eval not arg('hands') == 'none' and arg('arms'))">
     <rosparam>
       robot_hardware:
         - unique_robot_hw
@@ -117,7 +128,7 @@
   </group>
 
   <!-- Set rosparam, load xacros and launch state publisher nodes if only arms are being used (no hands) -->
-  <group if="$(eval arg('hands') == 'none' and arg('arm_ctrl'))">
+  <group if="$(eval arg('hands') == 'none' and arg('arms'))">
     <rosparam>
       robot_hardware:
         - ra_sr_ur_robot_hw
@@ -135,7 +146,7 @@
   </group>
 
   <!-- Set rosparams and load robot hardware if arms are being used -->
-  <group if="$(eval arg('arm_ctrl'))">
+  <group if="$(eval arg('arms'))">
     <rosparam command="load" file="$(arg arm_robot_hw_1)"/>
     <rosparam command="load" file="$(arg arm_robot_hw_2)"/>
     <rosparam command="load" file="$(arg kinematics_config_right)" ns="ra_sr_ur_robot_hw"/>
@@ -153,7 +164,7 @@
   </group>
 
   <!-- Set rosparam if only bimanual hands and no arms are being used -->
-  <group if="$(eval not arg('hands') == 'none' and not arg('arm_ctrl'))">
+  <group if="$(eval not arg('hands') == 'none' and not arg('arms'))">
     <rosparam>
       robot_hardware:
         - unique_robot_hw
diff --git a/sr_robot_launch/launch/sr_ur_arms_hands.launch b/sr_robot_launch/launch/sr_ur_arms_hands.launch
index d61fcabb3..242fdf845 100644
--- a/sr_robot_launch/launch/sr_ur_arms_hands.launch
+++ b/sr_robot_launch/launch/sr_ur_arms_hands.launch
@@ -34,6 +34,8 @@
   <!-- ROBOT CONFIG-->
   <!-- Specify if the system has "both" hands, only "right", only "left" or "none" hands-->
   <arg name="hands" default="both"/>
+  <arg name="left_hand" default="$(eval hands in ['left', 'both'])"/>
+  <arg name="right_hand" default="$(eval hands in ['right', 'both'])"/>
   <arg name="robot_model" default="ur10"/>
   <arg name="robot_description" default="'$(find sr_multi_description)/urdf/bimanual_srhand_$(arg robot_model).urdf.xacro'"/>
   <arg name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/bimanual_sh_$(arg robot_model).yaml"/>
@@ -52,11 +54,16 @@
   <!-- Set to true to spawn trajectory controllers for the hands(the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
   <arg name="hand_trajectory" default="true"/>
   <!-- Set to true to spawn the position controllers for the hands-->
-  <arg name="hand_ctrl" unless="$(eval hands == 'none')" default="true"/>
-  <arg name="hand_ctrl" if="$(eval hands == 'none')" value="false"/>
+  <arg name="hand_ctrl" default="$(eval hands != 'none')"/>
   <!-- The control mode PWM (true) or torque (false) -->
   <!-- Set to true by default for now as torque control is not available yet -->
   <arg name="pwm_control" default="true"/>
+  <!-- Set to true if you want to use grasp controller -->
+  <arg name="grasp_controller" default="false"/>
+  <!-- Default hand controller groups -->
+  <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
+  <arg if="$(eval hand_trajectory and not grasp_controller)" name="hand_controller_group" default="trajectory"/>
+  <arg if="$(eval not hand_trajectory and not grasp_controller)" name="hand_controller_group" default="position"/>
 
   <!-- ARMS CONFIG-->
   <arg name="arm_1_z" default="0.0"/>
@@ -67,7 +74,7 @@
   <arg name="arm_robot_hw_2" default="$(find sr_robot_launch)/config/left_bimanual_ur_arm_robot_hw.yaml"/>
   <!-- When cyberglove is used set "include_wrist_in_arm_controller:=false" as the wrist joints are part of the arm by default-->
   <!-- This will include the wrist joints in the hand controller and exclude them from the arm one -->
-  <arg name="include_wrist_in_arm_controller" default="true"/>
+  <arg name="include_wrist_in_arm_controller" default="$(eval hands != 'none')"/>
   <!-- Set to true to spawn trajectory controllers for the arms (the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
   <arg name="arm_trajectory" default="true"/>
   <!-- Set to true to spawn group position controllers for the arms -->
@@ -92,15 +99,17 @@
     </include>
     <param name="robot_description" command="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation)"/>
     <!-- HAND -->
-    <group if="$(eval hands == 'both' or hands == 'right')">
+    <group if="$(arg right_hand)">
       <param name="/hand/mapping/$(arg rh_serial)" value="rh"/>
       <param name="/hand/joint_prefix/$(arg rh_serial)" value="rh_"/>
-      <rosparam command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo.yaml"/>
+      <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo.yaml"/>
+      <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo_no_wrist.yaml"/>
     </group>
-    <group if="$(eval hands == 'both' or hands == 'left')">
+    <group if="$(arg left_hand)">
       <param name="/hand/mapping/$(arg lh_serial)" value="lh"/>
       <param name="/hand/joint_prefix/$(arg lh_serial)" value="lh_"/>
-      <rosparam command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo.yaml"/>
+      <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo.yaml"/>
+      <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo_no_wrist.yaml"/>
     </group>
     <arg if="$(arg start_home)" name="spawn_model_extra_args" value="-unpause $(arg home_angles)" />
     <arg unless="$(arg start_home)" name="spawn_model_extra_args" value="" />
@@ -111,10 +120,10 @@
     <!-- ARM -->
     <group if="$(arg arm_trajectory)">
       <group if="$(arg include_wrist_in_arm_controller)">
-        <rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller.yaml" command="load"/>
-        <rosparam if="$(eval hands == 'left'or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
-        <rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller.yaml" command="load"/>
-        <rosparam if="$(eval hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
+        <rosparam if="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller.yaml" command="load"/>
+        <rosparam unless="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
+        <rosparam if="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller.yaml" command="load"/>
+        <rosparam unless="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
       </group>
       <group unless="$(arg include_wrist_in_arm_controller)">
         <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
@@ -124,10 +133,10 @@
     </group>
     <group if="$(arg arm_position)">
       <group if="$(arg include_wrist_in_arm_controller)">
-        <rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller.yaml" command="load"/>
-        <rosparam if="$(hands == 'left' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
-        <rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller.yaml" command="load"/>
-        <rosparam if="$(hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
+        <rosparam if="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller.yaml" command="load"/>
+        <rosparam unless="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
+        <rosparam if="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller.yaml" command="load"/>
+        <rosparam unless="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
       </group>
       <group unless="$(arg include_wrist_in_arm_controller)">
         <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
@@ -148,9 +157,7 @@
     </group>
     <group if="$(arg hand_ctrl)">
       <node name="sr_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
-        <param if="$(arg hand_trajectory)" name="controller_group" value="trajectory"/>
-        <param unless="$(arg hand_trajectory)" name="controller_group" value="position"/>
-        <param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
+        <param name="controller_group" value="$(arg hand_controller_group)"/>
       </node>
     </group>
   </group>
@@ -165,6 +172,8 @@
         <arg name="robot_description" value="$(arg robot_description)"/>
         <arg name="robot_config_file" value="$(arg robot_config_file)"/>
         <arg name="hands" value="$(arg hands)"/>
+        <arg name="hand_ctrl" value="$(arg hand_ctrl)"/>
+        <arg name="hand_controller_group" value="$(arg hand_controller_group)"/>
         <arg name="rh_serial" value="$(arg rh_serial)"/>
         <arg name="lh_serial" value="$(arg lh_serial)"/>
         <arg name="rh_mapping_path" value="$(arg rh_mapping_path)"/>

From dff92cd4130c1ac97c379a84edf384717c6e4fcd Mon Sep 17 00:00:00 2001
From: Ethan Fowler <ethan@shadowrobot.com>
Date: Thu, 6 Aug 2020 12:58:39 +0000
Subject: [PATCH 2/5] WIP

---
 .../launch/sr_ur_arms_hands.launch            | 35 ++++++++-----------
 1 file changed, 14 insertions(+), 21 deletions(-)

diff --git a/sr_robot_launch/launch/sr_ur_arms_hands.launch b/sr_robot_launch/launch/sr_ur_arms_hands.launch
index 242fdf845..57cbd9d29 100644
--- a/sr_robot_launch/launch/sr_ur_arms_hands.launch
+++ b/sr_robot_launch/launch/sr_ur_arms_hands.launch
@@ -119,30 +119,22 @@
     </include>
     <!-- ARM -->
     <group if="$(arg arm_trajectory)">
-      <group if="$(arg include_wrist_in_arm_controller)">
-        <rosparam if="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller.yaml" command="load"/>
-        <rosparam unless="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
-        <rosparam if="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller.yaml" command="load"/>
-        <rosparam unless="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
-      </group>
-      <group unless="$(arg include_wrist_in_arm_controller)">
-        <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml" command="load"/>
-        <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/la_trajectory_controller_no_wrist.yaml" command="load"/>
-      </group>
+      <arg if="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_trajectory_controller_config" default="ra_trajectory_controller"/>
+      <arg if="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_trajectory_controller_config" default="la_trajectory_controller"/>
+      <arg unless="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_trajectory_controller_config" default="ra_trajectory_controller_no_wrist"/>
+      <arg unless="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_trajectory_controller_config" default="la_trajectory_controller_no_wrist"/>
+      <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg ra_gazebo_trajectory_controller_config).yaml" command="load"/>
+      <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg la_gazebo_trajectory_controller_config).yaml" command="load"/>
       <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_trajectory_controller la_trajectory_controller"/>
     </group>
     <group if="$(arg arm_position)">
-      <group if="$(arg include_wrist_in_arm_controller)">
-        <rosparam if="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller.yaml" command="load"/>
-        <rosparam unless="$(arg right_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
-        <rosparam if="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller.yaml" command="load"/>
-        <rosparam unless="$(arg left_hand)" file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
-      </group>
-      <group unless="$(arg include_wrist_in_arm_controller)">
-        <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/ra_group_position_controller_no_wrist.yaml" command="load"/>
-        <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/la_group_position_controller_no_wrist.yaml" command="load"/>
-      </group>
-      <node name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_trajectory_controller la_trajectory_controller"/>
+      <arg if="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_group_position_controller_config" default="ra_group_position_controller"/>
+      <arg if="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_group_position_controller_config" default="la_group_position_controller"/>
+      <arg unless="$(eval include_wrist_in_arm_controller and right_hand)" name="ra_gazebo_group_position_controller_config" default="ra_group_position_controller_no_wrist"/>
+      <arg unless="$(eval include_wrist_in_arm_controller and left_hand)" name="la_gazebo_group_position_controller_config" default="la_group_position_controller_no_wrist"/>
+      <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg ra_gazebo_group_position_controller_config).yaml" command="load"/>
+      <rosparam file="$(find sr_robot_launch)/config/gazebo/controller/$(arg la_gazebo_group_position_controller_config).yaml" command="load"/>
+      <node name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="ra_group_position_controller la_group_position_controller"/>
     </group>
     <!-- Robot state publisher -->
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
@@ -158,6 +150,7 @@
     <group if="$(arg hand_ctrl)">
       <node name="sr_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
         <param name="controller_group" value="$(arg hand_controller_group)"/>
+        <param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
       </node>
     </group>
   </group>

From 7738c30c90c72fa72379fa08c65a2ae07587ea87 Mon Sep 17 00:00:00 2001
From: Ethan Fowler <ethan@shadowrobot.com>
Date: Thu, 6 Aug 2020 16:31:53 +0000
Subject: [PATCH 3/5] Fixing controller spawning bug in which WRJ1+2 would not
 work when wrist was included in arm trajectory control

---
 sr_robot_launch/config/controller_spawner.yaml | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/sr_robot_launch/config/controller_spawner.yaml b/sr_robot_launch/config/controller_spawner.yaml
index 9b5c37696..fc1bac36a 100644
--- a/sr_robot_launch/config/controller_spawner.yaml
+++ b/sr_robot_launch/config/controller_spawner.yaml
@@ -99,6 +99,9 @@ controller_groups:
         - rh_trajectory_controller
       default:
         - sh_%joint_name%_position_controller
+      necessary_if_joint_present:
+        - sh_rh_wrj1_position_controller
+        - sh_rh_wrj2_position_controller
       rh_FFJ1:
         - sh_rh_ffj0_position_controller
       rh_FFJ2:
@@ -120,6 +123,9 @@ controller_groups:
         - lh_trajectory_controller
       default:
         - sh_%joint_name%_position_controller
+      necessary_if_joint_present:
+        - lh_wrj1_position_controller
+        - lh_wrj2_position_controller
       lh_FFJ1:
         - sh_lh_ffj0_position_controller
       lh_FFJ2:

From 3c8347bde1d6640f160f2af408b37bcdc3c9b021 Mon Sep 17 00:00:00 2001
From: Ethan Fowler <ethan@shadowrobot.com>
Date: Mon, 10 Aug 2020 13:43:37 +0000
Subject: [PATCH 4/5] WIP

---
 sr_robot_launch/launch/sr_ur_arms_hands.launch | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/sr_robot_launch/launch/sr_ur_arms_hands.launch b/sr_robot_launch/launch/sr_ur_arms_hands.launch
index 57cbd9d29..092a0b7d4 100644
--- a/sr_robot_launch/launch/sr_ur_arms_hands.launch
+++ b/sr_robot_launch/launch/sr_ur_arms_hands.launch
@@ -102,14 +102,12 @@
     <group if="$(arg right_hand)">
       <param name="/hand/mapping/$(arg rh_serial)" value="rh"/>
       <param name="/hand/joint_prefix/$(arg rh_serial)" value="rh_"/>
-      <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo.yaml"/>
-      <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo_no_wrist.yaml"/>
+      <rosparam command="load" file="$(find sr_description)/hand/config/rh_controller_gazebo.yaml"/>
     </group>
     <group if="$(arg left_hand)">
       <param name="/hand/mapping/$(arg lh_serial)" value="lh"/>
       <param name="/hand/joint_prefix/$(arg lh_serial)" value="lh_"/>
-      <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo.yaml"/>
-      <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo_no_wrist.yaml"/>
+      <rosparam command="load" file="$(find sr_description)/hand/config/lh_controller_gazebo.yaml"/>
     </group>
     <arg if="$(arg start_home)" name="spawn_model_extra_args" value="-unpause $(arg home_angles)" />
     <arg unless="$(arg start_home)" name="spawn_model_extra_args" value="" />

From e3cc3a1ba2665f64ae76b0ccc1022c87e92912f2 Mon Sep 17 00:00:00 2001
From: Ethan Fowler <ethan@shadowrobot.com>
Date: Mon, 10 Aug 2020 13:44:38 +0000
Subject: [PATCH 5/5] Fixing left hand controller spawner config

---
 sr_robot_launch/config/controller_spawner.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/sr_robot_launch/config/controller_spawner.yaml b/sr_robot_launch/config/controller_spawner.yaml
index fc1bac36a..ee07b46c6 100644
--- a/sr_robot_launch/config/controller_spawner.yaml
+++ b/sr_robot_launch/config/controller_spawner.yaml
@@ -124,8 +124,8 @@ controller_groups:
       default:
         - sh_%joint_name%_position_controller
       necessary_if_joint_present:
-        - lh_wrj1_position_controller
-        - lh_wrj2_position_controller
+        - sh_lh_wrj1_position_controller
+        - sh_lh_wrj2_position_controller
       lh_FFJ1:
         - sh_lh_ffj0_position_controller
       lh_FFJ2: