sr_interface/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch at ef98a28334b12ee8bae92d8c663eced97fa52075 · shadow-robot/sr_interface · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
<launch>
  <arg name="debug" default="false"/>
  <arg name="robot_state_pub_frequency" default="250"/>
  <arg name="joint_state_pub_frequency" default="125"/>

  <!-- 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 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('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 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 -->
  <arg name="load_robot_description" default="true"/>

  <!-- HANDS CONFIG-->
  <arg name="rh_serial" default="1370"/>
  <arg name="lh_serial" default="2346"/>
  <arg name="rh_mapping_path" default="$(find sr_edc_launch)/mappings/default_mappings/rh_E_v4.yaml"/>
  <arg name="lh_mapping_path" default="$(find sr_edc_launch)/mappings/default_mappings/lh_E_v4.yaml"/>
  <!-- Allows to specify the ethernet interface/s to be used. It defaults to the value of the env var ETHERCAT_PORT
       More than one interface can be specified by concatenating them using underscore as a separator (e.g eth1_eth2_eth3) -->
  <arg name="eth_port" default="$(optenv ETHERCAT_PORT eth0)"/>
  <!-- The control mode PWM (true) or torque (false) -->
  <arg name="pwm_control" default="true"/>
  <!-- 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('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" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/right_ur_arm_robot_hw.yaml"/>
  <arg name="arm_robot_hw_1" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/right_ur10e_arm_robot_hw.yaml"/>
  <arg name="arm_robot_hw_2" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/left_bimanual_ur_arm_robot_hw.yaml"/>
  <arg name="arm_robot_hw_2" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/left_bimanual_ur10e_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" if="$(arg arm_ctrl)"/>
  <arg name="include_wrist_in_arm_controller" default="false" unless="$(arg arm_ctrl)"/>
  <arg name="urcap_program_name" default="external_ctrl.urp"/>

  <arg name="arm_trajectory" default="true"/>
  <!-- Set to true to spawn the position controllers for the arm-->
  <arg name="arm_position" default="$(eval not arm_trajectory)"/>
  <arg name="right_arm_speed_scale" default="0.5"/>
  <arg name="left_arm_speed_scale" default="0.5"/>

  <arg unless="$(eval hands == 'none')" name="arm_payload_mass" default="4.55"/>
  <arg unless="$(eval hands == 'none')" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
  <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="kinematics_config_left" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find ur_description)/config/ur10_default.yaml"/>
  <arg name="kinematics_config_left" if="$(eval arg('robot_model') == 'ur10e')" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
  <arg name="kinematics_config_right" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find ur_description)/config/ur10_default.yaml"/>
  <arg name="kinematics_config_right" if="$(eval arg('robot_model') == 'ur10e')" default="$(find ur_e_description)/config/ur10e_default.yaml"/>

  <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) kinematics_config_right:=$(arg kinematics_config_right) kinematics_config_left:=$(arg kinematics_config_left)"/>

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

  <node pkg="rosservice" type="rosservice" name="set_right_arm_payload" args="call --wait /ra_sr_ur_robot_hw/set_payload '{payload: $(arg arm_payload_mass), center_of_gravity: $(arg arm_payload_cog)}'"/>
  <node pkg="rosservice" type="rosservice" name="set_left_arm_payload" args="call --wait /la_sr_ur_robot_hw/set_payload '{payload: $(arg arm_payload_mass), center_of_gravity: $(arg arm_payload_cog)}'"/>

  <!-- Logging local topics -->
  <group ns="control_box">
    <include file="$(find sr_logging_common)/launch/sr_rosbag_log.launch">
      <arg name="log_topics" value='-e ".*(controller|debug_etherCAT_data).*" /joint_states /diagnostics /mechanism_statistics'/>
      <arg name="log_bag_prefix" value="sr_hardware_control"/>
      <arg name="log_directory" value="$(optenv HOME)/.ros/log"/>
    </include>
  </group>

  <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 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-->
    <include file="$(find sr_edc_launch)/sr_edc_bimanual_ros_control.launch">
      <arg name="define_robot_hardware" value="false"/>
      <arg name="debug" value="$(arg debug)"/>
      <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)"/>
      <arg name="lh_mapping_path" value="$(arg lh_mapping_path)"/>
      <arg name="eth_port" value="$(arg eth_port)"/>
      <arg name="pwm_control" value="$(arg pwm_control)"/>
      <arg name="arm_1_z" value="$(arg arm_1_z)"/>
      <arg name="arm_2_z" value="$(arg arm_2_z)"/>
      <arg name="arm_x_separation" value="$(arg arm_x_separation)"/>
      <arg name="arm_y_separation" value="$(arg arm_y_separation)"/>
      <arg name="robot_state_pub_frequency" value="$(arg robot_state_pub_frequency)"/>
      <arg name="joint_state_pub_frequency" value="$(arg joint_state_pub_frequency)"/>
      <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 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('arms'))">
    <rosparam>
      robot_hardware:
        - unique_robot_hw
        - ra_sr_ur_robot_hw
        - la_sr_ur_robot_hw
    </rosparam>
  </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('arms'))">
    <rosparam>
      robot_hardware:
        - ra_sr_ur_robot_hw
        - la_sr_ur_robot_hw
    </rosparam>
    <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)"/>
    <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch">
      <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
    </include>
    <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
    <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)" />
    </node>
  </group>

  <!-- Set rosparams and load robot hardware if arms are being used -->
  <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"/>
    <rosparam command="load" file="$(arg kinematics_config_left)" ns="la_sr_ur_robot_hw"/>
    <param name="ra_sr_ur_robot_hw/speed_scale" type="double" value="$(arg right_arm_speed_scale)"/>
    <param name="la_sr_ur_robot_hw/speed_scale" type="double" value="$(arg left_arm_speed_scale)"/>
    <node name="sr_ur_arm_unlock" pkg="sr_robot_launch" type="sr_ur_arm_unlock" output="screen">
      <param name="urcap_program_name" value="$(arg urcap_program_name)"/>
    </node>
    <group if="$(eval 'e' in arg('robot_model'))">
      <rosparam command="load" file="$(find sr_robot_launch)/config/ra_force_torque_controller.yaml"/>
      <rosparam command="load" file="$(find sr_robot_launch)/config/la_force_torque_controller.yaml"/>
      <node name="ft_sensor_controllers_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="ra_force_torque_sensor_controller la_force_torque_sensor_controller" />
    </group>
  </group>

  <!-- Set rosparam if only bimanual hands and no arms are being used -->
  <group if="$(eval not arg('hands') == 'none' and not arg('arms'))">
    <rosparam>
      robot_hardware:
        - unique_robot_hw
    </rosparam>
  </group>

  <!-- Trajectory mode if arms are being used -->
  <group if="$(eval arg('arm_trajectory') and arg('arm_ctrl'))">
    <group if="$(arg include_wrist_in_arm_controller)">
      <rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/ra_trajectory_controller.yaml" command="load"/>
      <rosparam if="$(eval hands == 'left' or hands == 'none')" file="$(find sr_robot_launch)/config/ra_trajectory_controller_no_wrist.yaml" command="load"/>
      <rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/la_trajectory_controller.yaml" command="load"/>
      <rosparam if="$(eval hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/la_trajectory_controller_no_wrist.yaml" command="load"/>
    </group>
    <group unless="$(arg include_wrist_in_arm_controller)">
      <rosparam file="$(find sr_robot_launch)/config/ra_trajectory_controller_no_wrist.yaml" command="load"/>
      <rosparam file="$(find sr_robot_launch)/config/la_trajectory_controller_no_wrist.yaml" command="load"/>
    </group>
    <node name="ra_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
          args="--wait-for=/ra_sr_ur_robot_hw/robot_program_running ra_trajectory_controller"/>
    <node name="la_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
          args="--wait-for=/la_sr_ur_robot_hw/robot_program_running la_trajectory_controller"/>
  </group>

  <!-- Position mode if arms are being used -->
  <group if="$(eval arg('arm_position') and arg('arm_ctrl'))">
    <group if="$(arg include_wrist_in_arm_controller)">
      <rosparam if="$(eval hands == 'both' or hands == 'right')" file="$(find sr_robot_launch)/config/ra_group_position_controller.yaml" command="load"/>
      <rosparam if="$(eval hands == 'left' or hands == 'none')" file="$(find sr_robot_launch)/config/ra_group_position_controller_no_wrist.yaml" command="load"/>
      <rosparam if="$(eval hands == 'both' or hands == 'left')" file="$(find sr_robot_launch)/config/la_group_position_controller.yaml" command="load"/>
      <rosparam if="$(eval hands == 'right' or hands == 'none')" file="$(find sr_robot_launch)/config/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/ra_group_position_controller_no_wrist.yaml" command="load"/>
      <rosparam file="$(find sr_robot_launch)/config/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"/>
  </group>
</launch>