sr_interface/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch at c846ca14ffba174de32665b1f03d753e896f03ff · 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
<launch>
  <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="ur10e"/>
  <!-- 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"/>
  <!-- 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"/>
  <!-- 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"/>

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

  <!-- 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"/>
  <arg name="hand_trajectory" unless="$(eval hands == 'none')" default="true"/>
  <arg name="hand_trajectory" if="$(eval hands == 'none')" default="false"/>

  <!-- 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_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"/>
  <arg name="arm_compliance_control" default="false"/>
  <arg if="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/VelocityJointInterface"/>
  <arg unless="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/PositionJointInterface"/>
  <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="5.00"/>
  <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="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) transmission_hw_interface:=$(arg arm_transmission_interface)"/>


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

  <!-- 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">
    <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)"/>
    <param name="arm_2_z" value="$(arg arm_2_z)"/>
    <param name="arm_x_separation" value="$(arg arm_x_separation)"/>
    <param name="arm_y_separation" value="$(arg arm_y_separation)"/>
  </node>

  <!-- Controller -->
  <group unless="$(eval hands == 'none')">
    <!-- 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" 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)"/>
      <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)"/>
    </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="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'))">
    <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('arm_ctrl'))">
    <rosparam>
      robot_hardware:
        - ra_sr_ur_robot_hw
        - la_sr_ur_robot_hw
    </rosparam>
    <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 name="conditional_delayed_rostool_robot_state_publisher" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
      <param name="package_name" value="robot_state_publisher" />
      <param name="executable_name" value="robot_state_publisher" />
      <rosparam param="params_list">[/robot_description]</rosparam>
      <param name="launch_args_list" value="
      publish_frequency=$(arg robot_state_pub_frequency)" />
    </node>
  </group>

  <!-- Set rosparams and load robot hardware if arms are being used -->
  <group if="$(eval arg('arm_ctrl'))">
    <rosparam command="load" file="$(arg arm_robot_hw_1)"/>
    <rosparam command="load" file="$(arg arm_robot_hw_2)"/>
    <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('arm_ctrl'))">
    <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'))">
    <rosparam file="$(find sr_robot_launch)/config/ra_trajectory_controller.yaml" command="load"/>
    <rosparam file="$(find sr_robot_launch)/config/la_trajectory_controller.yaml" command="load"/>
    <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'))">
    <rosparam file="$(find sr_robot_launch)/config/ra_group_position_controller.yaml" command="load"/>
    <rosparam file="$(find sr_robot_launch)/config/la_group_position_controller.yaml" command="load"/>
    <node name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
          args="ra_trajectory_controller la_trajectory_controller"/>
  </group>
</launch>