-
Notifications
You must be signed in to change notification settings - Fork 164
/
spawn_kortex_robot.launch
170 lines (145 loc) · 9.22 KB
/
spawn_kortex_robot.launch
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
<launch>
<!-- Arguments -->
<!-- Start the GUIs -->
<arg name="start_gazebo" default="true"/>
<arg name="gazebo_gui" default ="true"/>
<arg name="start_rviz" default="true"/>
<!-- Initial position in Gazebo -->
<arg name="x0" default="0"/>
<arg name="y0" default="0"/>
<arg name="z0" default="0"/>
<!-- Arm type -->
<arg name="arm" default="gen3"/>
<arg name="dof" default="7" if="$(eval arg('arm') == 'gen3')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="dof" default="6" if="$(eval arg('arm') == 'gen3_lite')"/> <!-- Number of degrees of freedom of the arm -->
<arg name="vision" default="true"/> <!-- True if the arm has a Vision module -->
<!-- Default gripper for Gen3 is none, default gripper for Gen3 lite is gen3_lite_2f -->
<arg name="gripper" default="" if="$(eval arg('arm') == 'gen3')"/>
<arg name="gripper" default="gen3_lite_2f" if="$(eval arg('arm') == 'gen3_lite')"/>
<arg name="robot_name" default="my_$(arg arm)"/>
<arg name="prefix" default=""/>
<arg name="cyclic_data_publish_rate" default="40"/> <!--Hz-->
<!-- Gazebo parameters -->
<arg name="use_sim_time" default="true"/>
<arg name="debug" default="false" />
<arg name="paused" default="true"/>
<!-- Start Gazebo -->
<include file="$(find kortex_gazebo)/launch/start_gazebo.launch" if="$(arg start_gazebo)">
<arg name="gui" value="$(arg gazebo_gui)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- Delay before homing the arm -->
<arg name="start_delay_seconds" default="10"/>
<group ns="$(arg robot_name)">
<!-- Load the description for the robot -->
<!-- Without gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=true prefix:=$(arg prefix)"
if="$(eval not arg('gripper'))"/>
<!-- With gripper -->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/$(arg arm)_$(arg gripper).xacro dof:=$(arg dof) vision:=$(arg vision) sim:=true prefix:=$(arg prefix)"
unless="$(eval not arg('gripper'))"/>
<!-- Spawn the robot in Gazebo -->
<!-- <rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/gazebo_initial_joint_positions.yaml"/> -->
<!-- <param name="initial_positions" value=""/> TODO -->
<!-- Without gripper -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -param robot_description -model $(arg robot_name) -x $(arg x0) -y $(arg y0) -z $(arg z0)
-robot_namespace $(arg robot_name)
-J $(arg prefix)joint_1 1.57
-J $(arg prefix)joint_2 0.35
-J $(arg prefix)joint_3 3.14
-J $(arg prefix)joint_4 -2.00
-J $(arg prefix)joint_5 0
-J $(arg prefix)joint_6 -1.00
-J $(arg prefix)joint_7 1.57"
if="$(eval not arg('gripper'))"/> <!--TODO-->
<!-- With gripper -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -param robot_description -model $(arg robot_name) -x $(arg x0) -y $(arg y0) -z $(arg z0)
-robot_namespace $(arg robot_name)
-J $(arg prefix)joint_1 1.57
-J $(arg prefix)joint_2 0.35
-J $(arg prefix)joint_3 3.14
-J $(arg prefix)joint_4 -2.00
-J $(arg prefix)joint_5 0
-J $(arg prefix)joint_6 -1.00
-J $(arg prefix)joint_7 1.57"
unless="$(eval not arg('gripper'))"/> <!--TODO-->
<!-- Load controller configuration file from kortex_control package -->
<rosparam file="$(find kortex_control)/arms/$(arg arm)/$(arg dof)dof/config/joint_position_controllers.yaml" command="load" subst_value="true"/>
<rosparam file="$(find kortex_control)/grippers/$(arg gripper)/config/gripper_action_controller_parameters.yaml" command="load" subst_value="true"
unless="$(eval not arg('gripper'))"/>
<!-- Start the trajectory controllers -->
<!-- Without gripper -->
<node name="$(arg prefix)$(arg arm)_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="$(arg prefix)$(arg arm)_joint_trajectory_controller $(arg prefix)joint_state_controller"
if="$(eval not arg('gripper'))"/>
<!-- With gripper -->
<node name="$(arg prefix)$(arg arm)_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="$(arg prefix)$(arg arm)_joint_trajectory_controller $(arg prefix)joint_state_controller $(arg prefix)$(arg gripper)_gripper_controller"
unless="$(eval not arg('gripper'))"/>
<!-- Individual position controllers are stopped at first -->
<!-- For 6 DOF arms -->
<node name="$(arg prefix)$(arg arm)_position_controllers_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" if="$(eval arg('dof') == 6)" args=" --stopped
$(arg prefix)joint_1_position_controller
$(arg prefix)joint_2_position_controller
$(arg prefix)joint_3_position_controller
$(arg prefix)joint_4_position_controller
$(arg prefix)joint_5_position_controller
$(arg prefix)joint_6_position_controller"/>
<!-- For 7 DOF arms -->
<node name="$(arg prefix)$(arg arm)_position_controllers_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" if="$(eval arg('dof') == 7)" args=" --stopped
$(arg prefix)joint_1_position_controller
$(arg prefix)joint_2_position_controller
$(arg prefix)joint_3_position_controller
$(arg prefix)joint_4_position_controller
$(arg prefix)joint_5_position_controller
$(arg prefix)joint_6_position_controller
$(arg prefix)joint_7_position_controller"/>
<!-- Start simulated Kortex Driver -->
<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
<param name="sim" value="true"/>
<param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
<param name="arm" value="$(arg arm)"/>
<param name="gripper" value="$(arg gripper)"/>
<param name="dof" value="$(arg dof)"/>
<param name="robot_name" value="$(arg robot_name)"/>
<param name="prefix" value="$(arg prefix)"/>
<rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/joint_limits.yaml" subst_value="true"/>
<rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/twist_limits.yaml" subst_value="true"/>
<!-- If there is a gripper, load the active joint names for it -->
<rosparam command="load" file="$(find kortex_description)/grippers/$(arg gripper)/config/joint_limits.yaml" unless="$(eval not arg('gripper'))" subst_value="true"/>
</node>
<!-- Start robot state publisher -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen" />
<!-- Start MoveIt main executable -->
<!-- TODO Find cleaner way to do that and that will work with other arms -->
<!-- Without gripper -->
<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_move_it_config/launch/move_group.launch" if="$(eval not arg('gripper'))">
<arg name="dof" value="$(arg dof)"/>
<arg name="prefix" value="$(arg prefix)"/>
</include>
<!-- With gripper -->
<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_$(arg gripper)_move_it_config/launch/move_group.launch" unless="$(eval not arg('gripper'))">
<arg name="dof" value="$(arg dof)"/>
<arg name="prefix" value="$(arg prefix)"/>
</include>
<!-- Sleep, then unpause the physics in Gazebo and home the robot -->
<node name="home_the_arm" pkg="kortex_gazebo" type="home_robot.py" output="screen" respawn="false" launch-prefix="bash -c 'sleep $(arg start_delay_seconds); $0 $@'">
<param name="robot_name" value="$(arg robot_name)"/>
</node>
<!-- Test if homing the robot with MoveIt ended correctly -->
<test test-name="paramtest_gazebo_initialization" pkg="rostest" type="paramtest">
<param name="param_name_target" value="is_initialized" />
<param name="param_value_expected" value="true" />
<param name="wait_time" value="60" />
</test>
<!-- Start RViz -->
<node name="rviz" pkg="rviz" type="rviz" if="$(arg start_rviz)"/>
</group>
</launch>