From 271c48970eb567ac8a4e9a7a840163a095197f06 Mon Sep 17 00:00:00 2001
From: mikramarc <michal@shadowrobot.com>
Date: Thu, 15 Jul 2021 10:32:54 +0000
Subject: [PATCH 1/3] Removing loading of robot description at lower level by
 default

---
 .../sr_bimanual_hardware_control_loop.launch   |  7 +++++--
 .../launch/sr_hardware_control_loop.launch     | 18 +++++++++---------
 2 files changed, 14 insertions(+), 11 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 2b217c0fb..850d7aae7 100644
--- a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
@@ -19,7 +19,7 @@
 
   <!-- If the arm is available the automatic calibration script will load the robot_description,
        otherwise the robot description is loaded at lower level -->
-  <arg name="load_robot_description_at_lower_level" default="$(eval not arm_ctrl)"/>
+  <arg name="load_robot_description_at_lower_level" default="false"/>
 
   <!-- HANDS CONFIG-->
   <arg name="rh_serial" default="1370"/>
@@ -79,7 +79,7 @@
   <include file="$(find sr_robot_launch)/launch/bimanual_controller_stopper.launch"/>
 
   <!-- Constructs robot description string and loads it -->
-  <node unless="$(arg load_robot_description_at_lower_level)" name="construct_robot_description" pkg="sr_robot_launch" type="construct_robot_description" output="screen">
+  <node if="$(arg arm_ctrl)" name="construct_robot_description" pkg="sr_robot_launch" type="construct_robot_description" output="screen">
     <param name="arm_type" value="$(arg robot_model)"/>
     <param name="robot_description_file" value="$(arg robot_description)"/>
     <param name="arm_1_z" value="$(arg arm_1_z)"/>
@@ -88,6 +88,9 @@
     <param name="arm_y_separation" value="$(arg arm_y_separation)"/>
   </node>
 
+  <!-- Loads the robot description unless requested to be done at lower lever -->
+  <param if="$(eval not arg('arm_ctrl') and not arg('load_robot_description_at_lower_level'))" name="robot_description" command="$(arg load_robot_description_command)"/>
+
   <!-- Default hand controller groups -->
   <arg if="$(arg hybrid_controller)" name="hand_controller_group" default="hybrid"/>
   <arg if="$(eval hand_trajectory and not hybrid_controller)" name="hand_controller_group" default="trajectory"/>
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index cad22282c..32702b7d0 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -12,11 +12,8 @@
 
   <!-- Whether there is an arm. -->
   <arg name="arm" default="true"/>
-  
-  <!-- If the arm is available the automatic calibration script will load the robot_description,
-       otherwise the robot description is loaded at lower level -->
-  <arg name="load_robot_description_at_lower_level" value="$(eval not arm)"/>
-
+  <!-- Whether to load robot description at lower level. -->
+  <arg name="load_robot_description_at_lower_level" value="false"/>
   <!-- Whether there is a hand. -->
   <arg name="hand" default="true"/>
   <!-- Whether to run arm controllers. -->
@@ -33,17 +30,18 @@
   <arg if="$(eval arg('hand') and arg('hand_id')=='lh' and not arg('arm'))" name="robot_description" default="$(find sr_description)/robots/shadowhand_left_motor_plus.urdf.xacro"/>
   <arg if="$(eval arg('hand') and not arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh.yaml"/>
 
-  <!-- Robot config -->
+  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
+
   <!-- HANDS CONFIG-->
   <arg name="hand_serial" default="1082"/>
   <arg name="mapping_path" default="$(find sr_edc_launch)/mappings/default_mappings/$(arg hand_id)_E_v4.yaml"/>
   <arg name="eth_port" default="$(optenv ETHERCAT_PORT enp2s0)"/>
   <arg name="pwm_control" default="true"/>
+  <arg name="hand_trajectory" default="true"/>
   <!-- Set to true if you want to use grasp controller -->
   <arg name="grasp_controller" default="false"/>
   <!-- Set to true if you want to use hybrid controller -->
   <arg name="hybrid_controller" default="false"/>
-  <arg name="hand_trajectory" default="true"/>
 
   <!-- ARMS CONFIG-->
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
@@ -64,7 +62,6 @@
   <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
-  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_speed_scale" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_payload" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_payload '{payload: $(arg arm_payload_mass), center_of_gravity: $(arg arm_payload_cog)}'"/>
@@ -83,7 +80,7 @@
   </include>
 
   <!-- Constructs robot description string and loads it -->
-  <node unless="$(arg load_robot_description_at_lower_level)" name="construct_robot_description" pkg="sr_robot_launch" type="construct_robot_description" output="screen">
+  <node if="$(arg arm)" name="construct_robot_description" pkg="sr_robot_launch" type="construct_robot_description" output="screen">
     <param name="arm_type" value="$(arg robot_model)"/>
     <param name="robot_description_file" value="$(arg robot_description)"/>
     <param name="arm_x_separation" value="$(arg arm_x_separation)"/>
@@ -91,6 +88,9 @@
     <param name="initial_z" value="$(arg initial_z)"/>
     <param name="prefix" value="$(arg hand_id)_"/>
   </node>
+
+  <!-- Loads the robot description unless requested to be done at lower lever -->
+  <param if="$(eval not arg('arm') and not arg('load_robot_description_at_lower_level'))" name="robot_description" command="$(arg load_robot_description_command)"/>
   
   <!-- Default hand controller groups -->
   <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>

From cc8cfa0acb121d49c268eafdb16911d6aae21084 Mon Sep 17 00:00:00 2001
From: mikramarc <michal@shadowrobot.com>
Date: Thu, 15 Jul 2021 10:45:55 +0000
Subject: [PATCH 2/3] cleanup

---
 .../launch/sr_bimanual_hardware_control_loop.launch      | 9 ++++-----
 1 file changed, 4 insertions(+), 5 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 850d7aae7..5851c949b 100644
--- a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
@@ -5,6 +5,9 @@
   <!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
   <arg name="hands" default="both"/>
   <arg name="arm_ctrl" default="true"/>	
+  <!-- Whether to load robot description at lower level. -->
+  <arg name="load_robot_description_at_lower_level" default="false"/>
+
   <!-- ROBOT CONFIG-->
   <arg name="robot_model" default="ur10e"/>
   <!-- HANDS AND ARMS -->
@@ -17,9 +20,7 @@
   <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"/>
 
-  <!-- If the arm is available the automatic calibration script will load the robot_description,
-       otherwise the robot description is loaded at lower level -->
-  <arg name="load_robot_description_at_lower_level" default="false"/>
+  <arg name="load_robot_description_command" default="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)"/>
 
   <!-- HANDS CONFIG-->
   <arg name="rh_serial" default="1370"/>
@@ -59,8 +60,6 @@
   <arg if="$(eval hands == 'none')" name="arm_payload_mass" default="0.0"/>
   <arg if="$(eval hands == 'none')" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
 
-  <arg name="load_robot_description_command" default="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)"/>
-
   <node pkg="rosservice" type="rosservice" name="set_right_speed_scale" args="call --wait /ra_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg right_arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_left_speed_scale" args="call --wait /la_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg left_arm_speed_scale)'"/>
 

From a3874775d5f619d839013c2d3fed8d8e6733bd0e Mon Sep 17 00:00:00 2001
From: mikramarc <michal@shadowrobot.com>
Date: Thu, 15 Jul 2021 10:57:39 +0000
Subject: [PATCH 3/3] cleaning

---
 .../launch/sr_bimanual_hardware_control_loop.launch        | 7 ++-----
 sr_robot_launch/launch/sr_hardware_control_loop.launch     | 7 ++-----
 2 files changed, 4 insertions(+), 10 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 5851c949b..2f09836b6 100644
--- a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
@@ -5,8 +5,6 @@
   <!-- Specify if the system has "both" hands, only "right", only "left", or "none"-->
   <arg name="hands" default="both"/>
   <arg name="arm_ctrl" default="true"/>	
-  <!-- Whether to load robot description at lower level. -->
-  <arg name="load_robot_description_at_lower_level" default="false"/>
 
   <!-- ROBOT CONFIG-->
   <arg name="robot_model" default="ur10e"/>
@@ -88,7 +86,7 @@
   </node>
 
   <!-- Loads the robot description unless requested to be done at lower lever -->
-  <param if="$(eval not arg('arm_ctrl') and not arg('load_robot_description_at_lower_level'))" name="robot_description" command="$(arg load_robot_description_command)"/>
+  <param unless="$(arg arm_ctrl)" name="robot_description" command="$(arg load_robot_description_command)"/>
 
   <!-- Default hand controller groups -->
   <arg if="$(arg hybrid_controller)" name="hand_controller_group" default="hybrid"/>
@@ -102,9 +100,8 @@
     <!-- HAND (N.B. Arm robot harware is implicitly started here if ra_sr_ur_robot_hw is present in param /robot_hardware-->
     <include file="$(find sr_edc_launch)/sr_edc_bimanual_ros_control.launch">
       <arg name="define_robot_hardware" value="false"/>
+      <arg name="load_robot_description" value="false"/>
       <arg name="debug" value="$(arg debug)"/>
-      <arg name="load_robot_description" value="$(arg load_robot_description_at_lower_level)"/>
-      <arg name="load_robot_description_command" value="$(arg load_robot_description_command)"/>
       <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)"/>
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 32702b7d0..940b161b6 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -12,8 +12,6 @@
 
   <!-- Whether there is an arm. -->
   <arg name="arm" default="true"/>
-  <!-- Whether to load robot description at lower level. -->
-  <arg name="load_robot_description_at_lower_level" value="false"/>
   <!-- Whether there is a hand. -->
   <arg name="hand" default="true"/>
   <!-- Whether to run arm controllers. -->
@@ -90,7 +88,7 @@
   </node>
 
   <!-- Loads the robot description unless requested to be done at lower lever -->
-  <param if="$(eval not arg('arm') and not arg('load_robot_description_at_lower_level'))" name="robot_description" command="$(arg load_robot_description_command)"/>
+  <param unless="$(arg arm)" name="robot_description" command="$(arg load_robot_description_command)"/>
   
   <!-- Default hand controller groups -->
   <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
@@ -105,11 +103,10 @@
     <!-- HAND (N.B. Arm robot harware is implicitly started here if ra_sr_ur_robot_hw is present in param /robot_hardware-->
     <include file="$(find sr_edc_launch)/sr_edc_ros_control.launch">
       <arg name="define_robot_hardware" value="false"/>
+      <arg name="load_robot_description" value="false"/>
       <arg name="hand_robot_hardware_name" value="unique_robot_hw"/>
       <arg name="debug" value="$(arg debug)"/>
       <arg name="eth_port" value="$(arg eth_port)"/>
-      <arg name="load_robot_description" value="$(arg load_robot_description_at_lower_level)"/>
-      <arg name="load_robot_description_command" value="$(arg load_robot_description_command)"/>
       <arg name="pwm_control" value="$(arg pwm_control)"/>
       <arg name="hand_serial" value="$(arg hand_serial)"/>
       <arg name="hand_id" value="$(arg hand_id)"/>