system_bolt.ros2_control.xacro 8.65 KB
Newer Older
Paul Rouanet's avatar
Paul Rouanet committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="system_bolt" params="name prefix use_sim:=^|false use_fake_hardware:=^|true fake_sensor_commands:=^|false slowdown:=2.0">

    <ros2_control name="${name}" type="system">

      <xacro:if value="$(arg use_sim)">
        <hardware>
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
      </xacro:if>
      <xacro:unless value="$(arg use_sim)">
        <hardware>
          <xacro:if value="${use_fake_hardware}">
            <plugin>fake_components/GenericSystem</plugin>
            <param name="fake_sensor_commands">${fake_sensor_commands}</param>
            <param name="state_following_offset">0.0</param>
          </xacro:if>
          <xacro:unless value="${use_fake_hardware}">
            <plugin>ros2_hardware_interface_bolt/SystemBoltMultiInterfaceHardware</plugin>
            <param name="example_param_hw_start_duration_sec">2.0</param>
            <param name="example_param_hw_stop_duration_sec">3.0</param>
            <param name="example_param_hw_slowdown">${slowdown}</param>
Paul Rouanet's avatar
Paul Rouanet committed
25
            <param name="eth_interface">${enp3s0}</param>
Paul Rouanet's avatar
Paul Rouanet committed
26
27
28
29
            <param name="calib_kp">3.</param>
            <param name="calib_kd">0.05</param>
            <param name="calib_T">2.</param>
            <param name="calib_dt">0.001</param>
Paul Rouanet's avatar
Paul Rouanet committed
30
            </xacro:unless>
Paul Rouanet's avatar
Paul Rouanet committed
31
32
33
34
35
        </hardware>
      </xacro:unless>

      <joint name="FLHAA">
        <command_interface name="position">
Paul Rouanet's avatar
Paul Rouanet committed
36
          <param name="min">-0.5</param>
Paul Rouanet's avatar
Paul Rouanet committed
37
38
39
40
41
42
43
44
45
46
          <param name="max">0.5</param>
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <command_interface name="acceleration">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
Paul Rouanet's avatar
Paul Rouanet committed
47
48
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
49
50
51
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
52
53
        <state_interface name="kp"/>
        <state_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
54
        <param name="motor_number">0</param>
55
56
57
58
59
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
60
        <param name="motor_reversed_polarity">${true}</param>
Paul Rouanet's avatar
Paul Rouanet committed
61
        <param name="position_offset">0.238</param>
Paul Rouanet's avatar
Paul Rouanet committed
62
63
64
      </joint>
      <joint name="FLHFE">
        <command_interface name="position">
Paul Rouanet's avatar
Paul Rouanet committed
65
66
          <param name="min">-1.7</param>
          <param name="max">1.7</param>
Paul Rouanet's avatar
Paul Rouanet committed
67
68
69
70
71
72
73
74
75
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <command_interface name="acceleration">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
Paul Rouanet's avatar
Paul Rouanet committed
76
77
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
78
79
        <state_interface name="position"/>
        <state_interface name="velocity"/>
Paul Rouanet's avatar
Paul Rouanet committed
80
81
82
        <state_interface name="acceleration"/>
        <state_interface name="kp"/>
        <state_interface name="kd"/>
83
        <param name="motor_number">3</param>
84
85
86
87
88
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
89
        <param name="motor_reversed_polarity">${true}</param>
Paul Rouanet's avatar
Paul Rouanet committed
90
        <param name="position_offset">-0.308</param>
Paul Rouanet's avatar
Paul Rouanet committed
91
92
93
      </joint>
      <joint name="FLK">
        <command_interface name="position">
Paul Rouanet's avatar
Paul Rouanet committed
94
95
          <param name="min">-3.4</param>
          <param name="max">3.4</param>
Paul Rouanet's avatar
Paul Rouanet committed
96
97
98
99
100
101
102
103
104
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <command_interface name="acceleration">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
Paul Rouanet's avatar
Paul Rouanet committed
105
106
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
107
108
109
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
110
111
        <state_interface name="kp"/>
        <state_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
112
        <param name="motor_number">2</param>
113
114
115
116
117
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
118
        <param name="motor_reversed_polarity">${true}</param>
Paul Rouanet's avatar
Paul Rouanet committed
119
        <param name="position_offset">0.276</param>
Paul Rouanet's avatar
Paul Rouanet committed
120
121
122
123
      </joint>
      <joint name="FRHAA">
        <command_interface name="position">
          <param name="min">-0.5</param>
Paul Rouanet's avatar
Paul Rouanet committed
124
          <param name="max">0.5</param>
Paul Rouanet's avatar
Paul Rouanet committed
125
126
127
128
129
130
131
132
133
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <command_interface name="acceleration">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
Paul Rouanet's avatar
Paul Rouanet committed
134
135
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
136
137
138
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
139
140
        <state_interface name="kp"/>
        <state_interface name="kd"/>
141
        <param name="motor_number">1</param>
142
143
144
145
146
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
147
        <param name="motor_reversed_polarity">${true}</param>
Paul Rouanet's avatar
Paul Rouanet committed
148
        <param name="position_offset">-0.115</param>
Paul Rouanet's avatar
Paul Rouanet committed
149
150
151
      </joint>
      <joint name="FRHFE">
        <command_interface name="position">
Paul Rouanet's avatar
Paul Rouanet committed
152
153
          <param name="min">-1.7</param>
          <param name="max">1.7</param>
Paul Rouanet's avatar
Paul Rouanet committed
154
155
156
157
158
159
160
161
162
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
        <command_interface name="acceleration">
          <param name="min">-1</param>
          <param name="max">1</param>
        </command_interface>
Paul Rouanet's avatar
Paul Rouanet committed
163
164
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
165
166
167
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
168
169
        <state_interface name="kp"/>
        <state_interface name="kd"/>
170
        <param name="motor_number">5</param>
171
172
173
174
175
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
176
        <param name="motor_reversed_polarity">${false}</param>
Paul Rouanet's avatar
Paul Rouanet committed
177
        <param name="position_offset">-0.584</param>
Paul Rouanet's avatar
Paul Rouanet committed
178
179
180
      </joint>
      <joint name="FRK">
        <command_interface name="position">
Paul Rouanet's avatar
Paul Rouanet committed
181
182
          <param name="min">-3.4</param>
          <param name="max">3.4</param>
Paul Rouanet's avatar
Paul Rouanet committed
183
184
185
186
        </command_interface>
        <command_interface name="velocity">
          <param name="min">-1</param>
          <param name="max">1</param>
Paul Rouanet's avatar
Paul Rouanet committed
187
188
189
        </command_interface>
        <command_interface name="kp"/>
        <command_interface name="kd"/>
Paul Rouanet's avatar
Paul Rouanet committed
190
191
192
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <state_interface name="acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
193
194
        <state_interface name="kp"/>
        <state_interface name="kd"/>
195
        <param name="motor_number">4</param>
196
197
198
199
200
        <param name="gear_ratio">9.</param>
        <param name="motor_constant">0.025</param>
        <param name="max_current">12.</param>
        <param name="max_joint_velocity">80.</param>
        <param name="safety_damping">0.5</param>
201
        <param name="motor_reversed_polarity">${false}</param>
Paul Rouanet's avatar
Paul Rouanet committed
202
        <param name="position_offset">0.432</param>
Paul Rouanet's avatar
Paul Rouanet committed
203
      </joint>
Paul Rouanet's avatar
Paul Rouanet committed
204
      <sensor name="IMU">
Paul Rouanet's avatar
Paul Rouanet committed
205
206
        <state_interface name="gyroscope"/>
        <state_interface name="accelerometer"/>
Paul Rouanet's avatar
Paul Rouanet committed
207
        <state_interface name="linear_acceleration"/>
Paul Rouanet's avatar
Paul Rouanet committed
208
209
        <state_interface name="attitude_euler"/>
        <state_interface name="attitude_quaternion"/>
210
211
        <param name="rotate_vector">"1 2 3"</param>
        <param name="orientation_vector">"1 2 3 4"</param>
Paul Rouanet's avatar
Paul Rouanet committed
212
      </sensor>
Paul Rouanet's avatar
Paul Rouanet committed
213
214
215
216
217
218
    </ros2_control>

  </xacro:macro>

</robot>