-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathur5.launch
64 lines (52 loc) · 3.52 KB
/
ur5.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
<?xml version="1.0"?>
<launch>
<include file="$(find ur5_simulation)/launch/bench_test_world.launch"/>
<arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." />
<arg name="gripper_controller_type" default="position" doc="Choose for effort or position" />
<arg name="open_rviz" default="false" doc="The rviz allow for frames visualization"/>
<arg name="open_joint_trajectory_controller_gui" default="false" doc="Open rqt gui to control the joints"/>
<!-- send robot urdf to param server -->
<include file="$(find ur5_simulation)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
<arg name="ur5_transmission_hw_interface" value="hardware_interface/PositionJointInterface"/>
<arg name="eef_transmission_hw_interface" value="hardware_interface/PositionJointInterface"/>
</include>
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 1.02" respawn="false" output="screen" />
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<!-- start this controller -->
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<node name="joint_group_position_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn arm_controller" respawn="false" output="screen"/>
<rosparam file="$(find ur5_simulation)/config/gripper_controllers.yaml" command="load"/>
<node name="finger_joint_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn gripper_$(arg gripper_controller_type)_controller" respawn="false" output="screen"/>
<!-- load other controllers -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager"
respawn="false" output="screen" args="load joint_group_position_controller" />
<arg name="PI" value="$(eval pi)"/>
<arg name="PI_2" value="$(eval pi/2)"/>
<arg name="MINUS_PI_2" value="$(eval -pi/2)"/>
<node pkg="tf" type="static_transform_publisher"
name="base_link_new_base_linkbroadcaster" args="0 0 0 $(arg PI) 0 0 base_link dh_base_link 20" />
<node pkg="tf" type="static_transform_publisher"
name="shoulder_link1_broadcaster" args="0 0 0 $(arg PI) 0 0 shoulder_link link1 20" />
<node pkg="tf" type="static_transform_publisher"
name="shoulder_link2_broadcaster" args="0 0 0 $(arg PI) 0 $(arg PI_2) shoulder_link link2 20" />
<node pkg="tf" type="static_transform_publisher"
name="forearm_link3_broadcaster" args="0 0 0 $(arg PI_2) $(arg PI_2) 0 forearm_link link3 20" />
<node pkg="tf" type="static_transform_publisher"
name="wrist_2_link_link4_broadcaster" args="0 0 0 0 0 $(arg MINUS_PI_2) wrist_2_link link4 20" />
<node pkg="tf" type="static_transform_publisher"
name="wrist_3_link_link5_broadcaster" args="0 0 0 0 0 0 wrist_3_link link5 20" />
<node pkg="tf" type="static_transform_publisher"
name="ee_link_link6_broadcaster" args="0 0 0 $(arg MINUS_PI_2) 0 $(arg MINUS_PI_2) ee_link link6 20" />
<group if="$(arg open_rviz)">
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ur5_simulation)/config/ur5.rviz" required="true" />
</group>
<group if="$(arg open_joint_trajectory_controller_gui)">
<node name="rqt_joint_trajectory_controller" pkg="rqt_joint_trajectory_controller"
type="rqt_joint_trajectory_controller" required="true" output="screen"/>
</group>
</launch>