-
Notifications
You must be signed in to change notification settings - Fork 34
/
lbr.ros2_control.xacro
124 lines (121 loc) · 5.62 KB
/
lbr.ros2_control.xacro
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
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="lbr_hardware_interface" params="sim:=^|true remote_host:=^|INADDR_ANY port:=^|30200 robot_name:=^|lbr">
<!-- arg for control mode -->
<ros2_control name="lbr_hardware_interface" type="system">
<!-- define hardware including parameters, also gazebo -->
<xacro:if value="${sim}">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
</xacro:if>
<xacro:unless value="${sim}">
<hardware>
<plugin>LBR::FRIHardwareInterface</plugin>
<param name="remote_host">${remote_host}</param>
<param name="port">${port}</param>
</hardware>
</xacro:unless>
<!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
<sensor name="${robot_name}_state">
<!-- see KUKA::FRI::LBRState -->
<state_interface name="sample_time"/>
<state_interface name="time_stamp_sec"/>
<state_interface name="time_stamp_nano_sec"/>
</sensor>
<!-- define joints and command/state interfaces for each joint -->
<joint name="${robot_name}_joint_0">
<command_interface name="position">
<!-- better to use radians as min max first -->
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_3">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_4">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_5">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
<joint name="${robot_name}_joint_6">
<command_interface name="position">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-1</param>
<param name="max"> 1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="effort"/>
<state_interface name="external_torque"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>