sr_interface/sr_robot_launch/launch/sr_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
195
196
197
198
199
200
<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="ur10e"/>
  <arg name="side" default="right"/>
  <arg name="side_letter" value="$(eval side[0])"/>
  <arg name="hand_id" value="$(arg side_letter)h"/>
  <arg name="arm_prefix" value="$(arg side_letter)a"/>

  <!-- 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 there is a hand. -->
  <arg name="hand" default="true"/>
  <!-- Whether to run arm controllers. -->
  <arg name="arm_ctrl" default="$(arg arm)"/>
  <!-- Whether to run hand controllers. -->
  <arg name="hand_ctrl" default="$(arg hand)"/>

  <!-- Robot description -->
  <arg if="$(eval arg('hand') and arg('arm'))" name="robot_description" default="$(find sr_multi_description)/urdf/$(arg side)_srhand_ur10_bt_sp_biotacs.urdf.xacro"/>
  <arg if="$(eval arg('hand') and arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh_$(arg robot_model).yaml"/>
  <arg if="$(eval not arg('hand') and arg('arm'))" name="robot_description" default="$(find sr_box_ur10_moveit_config)/config/$(arg side_letter)a_$(arg robot_model)_with_box.urdf.xacro"/>
  <arg if="$(eval not arg('hand') and arg('arm'))" name="robot_config_file" default="$(find sr_multi_moveit_config)/config/robot_configs/$(arg side)_sh_$(arg robot_model).yaml"/>
  <arg if="$(eval arg('hand') and arg('hand_id')=='rh' and not arg('arm'))" name="robot_description" default="$(find sr_description)/robots/shadowhand_motor_plus.urdf.xacro"/>
  <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 -->
  <!-- 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"/>
  <!-- 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="arm_compliance_control" default="true"/>
  <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
  <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
  <arg name="arm_x_separation" default="0.0"/>
  <arg name="arm_y_separation" default="0.0"/>
  <arg name="arm_robot_hw" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur_arm_robot_hw.yaml"/>
  <arg name="arm_robot_hw" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur10e_arm_robot_hw.yaml"/>
  <arg name="force_torque_sensor_hw" default="$(find sr_robot_launch)/config/$(arg arm_prefix)_force_torque_controller.yaml"/>
  <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="arm_speed_scale" default="0.5"/>
  <arg if="$(arg hand)" name="arm_payload_mass" default="5.00"/>
  <arg if="$(arg hand)" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
  <arg unless="$(arg hand)" name="arm_payload_mass" default="0.0"/>
  <arg unless="$(arg hand)" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
  <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="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 if="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
  <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_trajectory_controller.yaml"/>
  <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) transmission_hw_interface:=$(arg arm_transmission_interface)"/>

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

  <!-- 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/unimanual_controller_stopper.launch">
    <arg name="side" value="$(arg side)"/>
  </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">
    <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)"/>
    <param name="arm_y_separation" value="$(arg arm_y_separation)"/>
    <param name="initial_z" value="$(arg initial_z)"/>
    <param name="prefix" value="$(arg hand_id)_"/>
  </node>

  <!-- Default hand controller groups -->
  <arg if="$(arg grasp_controller)" name="hand_controller_group" default="grasp"/>
  <arg if="$(arg hybrid_controller)" name="hand_controller_group" default="hybrid"/>
  <arg if="$(eval hand_trajectory and not grasp_controller and not hybrid_controller)" name="hand_controller_group" default="trajectory"/>
  <arg if="$(eval not hand_trajectory and not grasp_controller and not hybrid_controller)" name="hand_controller_group" default="position"/>

  <!-- Controller -->
  <group if="$(arg hand)">
    <!-- 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_ros_control.launch">
      <arg name="define_robot_hardware" 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)"/>
      <arg name="mapping_path" value="$(arg mapping_path)"/>
      <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="initial_z" value="$(arg initial_z)"/>
      <arg name="arm_x_separation" value="$(arg arm_x_separation)"/>
      <arg name="arm_y_separation" value="$(arg arm_y_separation)"/>
    </include>
    <node if="$(arg hand_ctrl)" name="$(arg hand_id)_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
      <param name="controller_group" value="$(arg hand_controller_group)"/>
      <param name="wait_for" value ="calibrated"/>
    </node>
  </group>

  <group if="$(arg arm_ctrl)">
    <group if="$(arg hand_ctrl)">
      <rosparam if="$(eval arg('side') == 'right')">
        robot_hardware:
          - unique_robot_hw
          - ra_sr_ur_robot_hw
      </rosparam>
      <rosparam if="$(eval arg('side') == 'left')">
        robot_hardware:
          - unique_robot_hw
          - la_sr_ur_robot_hw
      </rosparam>
    </group>
    <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>
    <rosparam unless="$(arg hand_ctrl)" param="robot_hardware" subst_value="True">[$(arg arm_prefix)_sr_ur_robot_hw]</rosparam>
    <rosparam command="load" file="$(arg arm_robot_hw)"/>
    <param name="$(arg arm_prefix)_sr_ur_robot_hw/speed_scale" type="double" value="$(arg arm_speed_scale)"/>
    <group if="$(eval 'e' in arg('robot_model'))">
      <rosparam command="load" file="$(arg force_torque_sensor_hw)"/>
      <node name="ft_sensor_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg arm_prefix)_force_torque_sensor_controller" />
    </group>
  </group>

  <rosparam unless="$(arg arm_ctrl)">
    robot_hardware:
      - unique_robot_hw
  </rosparam>

  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
    <param name="package_name" value="wrench_to_joint_vel_pub" />
    <param name="executable_name" value="ur_compliance.launch" />
    <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
    <param name="timeout" value="30.0" />
  </node>

  <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
  <group unless="$(arg hand)">
    <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"/>

  </group>
  <!-- These will be loaded if arm and hand are enabled -->
  <!-- Trajectory mode -->
  <group if="$(arg arm_ctrl)">
    <group if="$(arg arm_trajectory)">
      <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_config)"/>
      <node unless="$(arg hand_ctrl)" name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
    </group>
    <!-- Position mode -->
    <group if="$(arg arm_position)">
      <group if="$(arg hand_ctrl)">
        <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller.yaml"/>
      </group>
      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller.yaml"/>
      <node if="$(arg hand_ctrl)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_arm_ready $(arg arm_prefix)_group_position_controller"/>
      <node unless="$(arg hand_ctrl)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
            args="--wait-for=/$(arg arm_prefix)_arm_ready $(arg arm_prefix)_group_position_controller"/>
    </group>
  </group>
</launch>