sr_interface/sr_robot_launch/launch/sr_ur_arms_hands.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
218
219
220
221
222
<launch>
  <!-- This launch file is for a bimanual system of ur arms and shadow hands -->

  <!-- set to false if you want to use real robots, true if you need the gazebo simulation -->
  <arg name="sim" default="true"/>
  <!-- Set to true for debugging -->
  <arg name="debug" default="false"/>
  <!-- Set to true to allow planning trajectories-->
  <arg name="use_moveit" default="true"/>
  <!--Choose planner library, options are ompl, stomp or sbpl-->
  <arg name="planning" default="ompl"/>
  <!--Should the planner yaml be automatically generated? -->
  <arg name="generate_planning_config" default="true"/>
  <!-- Set to false if you want to start without guis -->
  <arg name="gui" default="true"/>
  <arg name="verbose" default="true" />
  <!-- Set whether to load a scene to restrain the movement of the robot -->
  <arg name="scene" default="false"/>
  <!-- Set the scene file -->
  <arg name="scene_file" default="$(find sr_description_common)/scenes/bimanual_demo_space_small.scene"/>
  <!-- Set world value -->
  <arg name="world" default="$(find sr_description_common)/worlds/shadowhands_and_arms.world" />
  <!-- Set to true if you want the robot to start in the "home" positions -->
  <arg name="start_home" default="false"/>
  <!-- Definition of "home" position, in joint-angle pairs -->
  <arg name="home_angles" default="-J la_shoulder_pan_joint 0.0 -J la_elbow_joint -2.0 -J la_wrist_1_joint -2.4 -J la_wrist_2_joint -1.5708 -J la_shoulder_lift_joint -1.89 -J la_wrist_3_joint 3.1416 -J ra_shoulder_pan_joint 0.0 -J ra_elbow_joint 2.0 -J ra_wrist_1_joint -0.733 -J ra_wrist_2_joint 1.5708 -J ra_shoulder_lift_joint -1.25 -J ra_wrist_3_joint 3.1416"/>
  <!-- Set this to true if you run the hardware loop on a remote computer -->
  <arg name="external_control_loop" default="false"/>
  <!-- The robot state publisher publishing frequency  -->
  <arg name="robot_state_pub_frequency" default="250"/>
  <!-- The joint state publisher publishing frequency  -->
  <arg name="joint_state_pub_frequency" default="125"/>

  <!-- 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"/>
  <arg name="robot_name" default="$(arg robot_model)srh"/>

  <!-- HANDS CONFIG-->
  <arg name="rh_serial" default="1290"/>
  <arg name="lh_serial" default="1338"/>
  <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 for ethercat devices. 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)"/>
  <!-- ik solver possible values sr_hand_kinematics/trac_ik/bio_ik -->
  <arg name="hand_ik_solver" default="sr_hand_kinematics"/>
  <!-- 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" 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"/>
  <arg name="arm_2_z" default="0.0"/>
  <arg name="arm_x_separation" default="1.0"/>
  <arg name="arm_y_separation" default="1.0"/>
  <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="$(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 -->
  <arg name="arm_position" default="$(eval not arm_trajectory)"/>
  <!-- Set to true to spawn the position controllers for the arm-->
  <arg name="arm_ctrl" default="true"/>
  <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]"/>

  <!-- SIMULATED ROBOTS -->
  <group if="$(arg sim)">
    <env name="GAZEBO_MODEL_PATH" value="$(find sr_description_common)/models/" />
    <env name="GAZEBO_MODEL_DATABASE_URI" value="" />
    <env name="MESH_WORKSPACE_PATH" value="$(find sr_description_common)/models/" />
    <arg name="paused" value="$(arg start_home)"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
      <arg name="world_name" value="$(arg world)"/>
      <arg name="gui" value="$(arg gui)"/>
      <arg name="paused" value="$(arg paused)"/>
      <arg name="debug" value="$(arg debug)"/>
      <arg name="verbose" value="$(arg verbose)" />
    </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="$(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"/>
    </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 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="" />
    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model $(arg robot_name) $(arg spawn_model_extra_args)" respawn="false" output="screen"/>
    <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch" >
      <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
    </include>
    <!-- ARM -->
    <group if="$(arg arm_trajectory)">
      <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)">
      <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">
      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)"/>
      <param name="tf_prefix" type="string" value="" />
    </node>
    <!-- Set scene for robot - restrains movement -->
    <group if="$(arg scene)">
      <include file="$(find gazebo2rviz)/launch/gazebo2moveit.launch">
        <arg name="frequency" value="1" />
      </include>
    </group>
    <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>

  <!-- REAL ROBOTS -->
  <group unless="$(arg sim)">
    <group unless="$(arg external_control_loop)">
      <include file="$(find sr_robot_launch)/launch/sr_bimanual_hardware_control_loop.launch">
        <arg name="debug" value="$(arg debug)"/>
        <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="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)"/>
        <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="hand_trajectory" value="$(arg hand_trajectory)"/>
        <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="arm_robot_hw_1" value="$(arg arm_robot_hw_1)"/>
        <arg name="arm_robot_hw_2" value="$(arg arm_robot_hw_2)"/>
        <arg name="include_wrist_in_arm_controller" value="$(arg include_wrist_in_arm_controller)"/>
        <arg name="arm_trajectory" value="$(arg arm_trajectory)"/>
        <arg name="arm_position" value="$(arg arm_position)"/>
        <arg name="right_arm_speed_scale" value="$(arg right_arm_speed_scale)"/>
        <arg name="left_arm_speed_scale" value="$(arg left_arm_speed_scale)"/>
        <arg name="arm_payload_mass" value="$(arg arm_payload_mass)"/>
        <arg name="arm_payload_cog" value="$(arg arm_payload_cog)"/>
      </include>
    </group>
    <!-- Set scene for robot after delay to start after moveit - restrains movement -->
    <group if="$(arg scene)">
      <node name="conditional_delayed_rostool_bimanual_scene" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
        <param name="package_name" value="sr_multi_moveit_config" />
        <param name="executable_name" value="scene.launch" />
        <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
        <param name="launch_args_list" value="scene_file:=$(arg scene_file)" />
        <param name="timeout" value="20.0" />
      </node>
    </group>
  </group>

  <!-- MOVEIT -->
  <group if="$(arg use_moveit)">
    <rosparam file="$(arg robot_config_file)"/>
      <include file="$(find sr_multi_moveit_config)/launch/moveit_planning_and_execution.launch">
        <arg name="hand_ik_solver" value="$(arg hand_ik_solver)"/>
        <arg name="load_robot_description" value="true"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="robot_config_file" value ="$(arg robot_config_file)"/>
        <arg name="planning" value="$(arg planning)"/>
        <arg name="generate_planning_config" value="$(arg generate_planning_config)"/>
      </include>
      <include file="$(find sr_multi_moveit_config)/launch/default_warehouse_db.launch"/>
  </group>
</launch>