ROS仿真揭秘——七自由度机器人(三)


进来聊一聊URDF啊~

1. 概述

先放出官方文件镇楼:

  1. ROS wiki urdf Tutorials.
  2. ROS wiki xacro
  3. ROS wiki urdf
  4. ROS wiki URDF-XML

做机器人仿真的第一步是设计相应的模型。机器人模型可以通过AutoCAD、SolidWorks、Blender这些CAD工具来进行设计。ROS有一个用来设计和创造机器人模型的标准的元包,叫做robot_model,它由urdf, kdl_parser, robot_state_publisher, 和collada_urdf等软件包组成。在ROS的仿真中,我们可以用.urdf文件、.xacro文件、.sdf文件来描述机器人的模型。三者都是XML格式的描述文件,通过一个个的link、joint等标签将机器人模型搭建起来。但是三者又有一些区别。.xacro文件是对.urdf文件的一个简化,它具有宏定义以及数学运算的功能,因此在多个关节的时候可以简化文件描述的长度。据我目前了解到的,.sdf文件应该是Gazebo最终读取的格式,即使是.urdf格式,它也会被自动转换成.sdf文件来读取(这里如有错误,欢迎尽早告知)。目前,我们可以将.urdf文件和.xacro文件统称为URDF文件。并且,.xacro文件可以转为.urdf文件,.urdf文件可以转为.sdf文件(虽然到目前为止,我得到的转换效果很差)。

从上一篇博客中,我们看到,七自由度机器人的3D仿真模型包mastering_ros_description_pkg和Gazebo仿真模型包seven_dof_arm_gazebo的交汇点就是mastering_ros_description_pkg/urdf/seven_dof_arm_gazebo.xacro文件。由于该文件源码比较长,放前面影响阅读体验,我将它放在了本篇博客的后面。接下来我们会选取它的组成部分进行解读。

2. 理解URDF文件——seven_dof_arm.xacro

URDF可以表示机器人的运动学和动态描述,机器人的视觉表示以及机器人的碰撞模型。用URDF文件来描述机器人用到的标签有:link、joint、robot、gazebo。用URDF文件来描述机器人的基本思想是:机器人每个关节都是一个link,link和link之间的链接方式是joint。

2.1 robot标签

<robot>...</robot>标签定义了我们要创建的机器人的名称,这里即seven_dof_arm。七自由度机器人的其他描述内容都会包含在该标签内。

1
2
3
4
<?xml version="1.0"?>
<robot name="seven_dof_arm" xmlns:xacro="http://ros.org/wiki/xacro">
...
</robot>

2.2 material标签

<material>...</material>标签定义了整个七自由度机器人的颜色组成,方便之后调用。

1
2
3
<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>

2.3 property标签

我们可以使用 property 标签声明作为 xacro 文件中的命名值的常量或 ropers,可以在代码中的任何位置使用。这些常量定义的主要用途是,我们不是在 link 和 joint 上给出数值,而是可以保留常量,并且可以在开头就修改掉相应的数值。此处给出了使用 property 的示例。

1
2
3
4
5
6
7
8
9
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>

<!-- Constants -->
<xacro:property name="M_SCALE" value="0.001 0.001 0.001"/>
<xacro:property name="M_PI" value="3.14159"/>

<!-- Shoulder pan link properties -->
<xacro:property name="shoulder_pan_width" value="0.04" />
<xacro:property name="shoulder_pan_len" value="0.08" />

这样,在之后需要使用到这些 property 的时候,只需要像下面的示例调用即可,property 有点类似于变量。

1
<cylinder radius="${shoulder_pan_width}" length="${shoulder_pan_len}"/>

2.4 macro标签

macro 标签可以作为宏定义来使用。给出 macro 的一个示例,如下所示:

1
2
3
4
5
6
<xacro:macro name="inertial_matrix" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>

这里使用 macro 定义了<inertial>标签,该标签中参数的 mass 定义了质量, inertia 定义了相应几何体的惯性张量。这样,在后面就无需对每个 link 的 inertial 属性进行设置,提高代码复用率。

2.5 link标签

link 标签的官方解释见ROS wiki link<link>...</link>标签表示机器人的单个链接。 使用这个标签,我们可以对机器人链接及其属性进行建模。 建模包括大小,形状和颜色,甚至可以导入3D网格来表示机器人链接。 我们还可以提供链接的动态属性,例如惯性矩阵和碰撞属性。对于本文的七自由度机器人,总共有12个link,如下表所示。

No. name No. name
1 bottom_link 7 wrist_roll_link
2 base_link 8 wrist_pitch_link
3 shoulder_pan_link 9 gripper_roll_link
4 shoulder_pitch_link 10 gripper_finger_link1
5 elbow_roll_link 11 gripper_finger_link2
6 elbow_pitch_link 12 grasping_frame

上表中,第1个 bottom_link 是将七自由度机器人固定的基座,2-9是从底向上组成七自由度机器人的8个link,10-11是七自由度机器人末端的二指夹爪,第12个是二指夹爪中间的一个小方块。

描述 link 的内容可以分为visualcollisioninertial三大类。这里我们给出 base_link 的具体内容作为示例。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
<link name="base_link">

<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<!-- rotate PI/2 -->
<geometry>
<box size="0.1 0.1 0.1" />
</geometry>
<material name="White" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<!-- rotate PI/2 -->
<geometry>
<box size="0.1 0.1 0.1" />
</geometry>
</collision>>
<xacro:inertial_matrix mass="1"/>

</link>

解释:

  • visual 标签定义了底座的外观,它是在仿真时显示出来的形态。origin 标签定义了 link 的起始位置,并且绕 x 轴旋转了 M_PI/2。geometry 标签定义了该 link 的几何形状为长方体 box,geometry 标签可定义的形状包括 cylinder, box, sphere, mesh。material 标签定义了 link 的颜色为白色,这在第2.2节中是有过定义的。
  • collision 标签定义了机器人在仿真过程中实际的物理特性:真实的几何形状及大小、初始位置。一般情况下,都要保证 visual 标签和 collision标签的属性一致。
  • inertial 标签定义了 link 的质量以及惯性张量。这里就直接调用了第2.4节中对于 inertial 的宏定义,传入的参数是 1。

2.6 joint标签

joint 标签的官方解释见ROS wiki joint<joint>...</joint>标签是用来链接两个 link 的,可以理解为一个 joint 就是机器人的一个电机。joint 会包含机器人的摩擦等动力学参数。七自由度机器人包含的 joint 及其特性如下表所示。

No. Joint name Joint type Angle limits(in degree)
1 bottom_joint fixed —-
2 shoulder_pan_joint revolute [-150, 114]
3 shoulder_pitch_joint revolute [-67, 109]
4 elbow_roll_joint revolute [-150, 41]
5 elbow_pitch_joint revolute [-92, 110]
6 wrist_roll_joint revolute [-150, 150]
7 wrist_pitch_joint revolute [92, 113]
8 gripper_roll_joint revolute [150, 150]
9 gripper_finger_joint1 prismatic [0, 3] cm
10 gripper_finger_joint2 prismatic [0, 3] cm

说明:在上表中, Joint type 给出了三种形式:fixed(固定)、revolute(旋转)、prismatic(平移)。对于 bottom_joint,我们是希望机器人的底座是固定在基座上的,它不可旋转;对于2-8是机器人的七个关节,是可以旋转的;9-10是二指夹爪的两个关节,可以平移。

选取一个 joint 作为示例,如下:

1
2
3
4
5
6
7
8
9
<joint name="finger_joint1" type="prismatic">
<parent link="gripper_roll_link"/>
<child link="gripper_finger_link1"/>
<origin xyz="0.0 0 0" />
<axis xyz="0 1 0" />
<limit effort="100" lower="0" upper="0.03" velocity="1.0"/>
<safety_controller k_position="20" k_velocity="20" soft_lower_limit="${-0.15 }" soft_upper_limit="${ 0.0 }"/>
<dynamics damping="50" friction="1"/>
</joint>

<joint>...</joint>标签内的元素有 name, type, parent link, child link, origin, axis, limit, safety_controller, dynamics 等。其中有的元素是必须的,有的不是必须的。

  • name:指定该 joint 的名字为 finger_joint1,这是末端夹爪的一根手指关节。
  • type:指定该关节类型为 prismatic,可以指定的 type 共有6种:
    • continuous: 一个不受限制的, 绕着一根轴的转动副
    • revolute: 一个转动角度受到限制的, 绕着一根轴的转动副
    • prismatic: 一个沿着一根轴的滑动副, 并且有限位
    • fixed: 固定关节
    • floating: 这个关节允许六个自由度的运动, 浮动关节
    • planar: This joint allows motion in a plane perpendicular to the axis. 这个关节允许再垂直于轴的一个平面内进行运动,这里的运动应该即包括平移和旋转
  • parent (必须):指定关节起始的 link。
  • child (必须):指定关节终止的 link。
  • origin (可选,默认为0):它反应了从 parent link 到 child link 之间的一个变换。关节落在 child link 的原点。也就是说, 我在确认了 parent link 和 child link 自己的坐标系之后, 还没有确认他们之间的关系。为了描述这个关系, 以 parent link 的坐标系为一个基础, 在上面描述出 child link 的坐标系原点的位置。那么这个当中包含了两个属性:
    • xyz (可选,默认为一个零向量): 这其实反映了两个坐标系之间的平移关系.
    • rpy (可选的,默认也是一个零向量): 这反映了两个坐标系之间旋转的一个关系, r 代表了 roll, 是绕着 x 轴旋转, p 代表了 pitch, 是绕着 y 轴旋转, y代表了yaw, 是绕着 z 轴旋转。
  • axis (可选, 默认为(1, 0, 0)):相对于child link也就是joint link的坐标系。对于continuous或者revolute关节,反映了绕着哪一根轴旋转;对于prismatic关节,反映了沿着哪一根轴移动;对于planar关节,反映了沿着哪个平面(由法向量体现)移动。
  • limit (仅仅在prismatic和revolute的关节时需要设置)。其中包含如下几个属性:
    • lower 和 upper (可选, 默认为0): 反映了关节移动的最小值或者最大值。如果关节是 continuous 类型的话, 就会被自动忽略。如果不填的话全部默认为0。
    • effort:最大关节力矩。要求(|applied effort| < |effort|)。
    • velocity:最大速度。
  • safety_controller (可选)。包含如下属性:
    • soft_lower_limit:指定安全控制器开始限制关节位置的下部关节边界的属性。该值要大于lower。
    • soft_upper_limit:指定安全控制器开始限制关节位置的上部关节边界的属性。该值要小于upper。
    • k_position:指定位置和速度之间的限制关系。
    • k_velocity:指定力矩和速度之间的限制关系。
    • 官方以pr2机器人的Joint Safety Limits Explained为例介绍了这一属性。

2.7 gazebo标签

官方介绍见Tutorial: Using a URDF in Gazebo。该标签可以使模型在Gazebo中正常显示。在七自由度机器人的URDF文件中使用 gazebo 标签为相关的 link 添加了颜色属性。

1
2
3
<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>

2.8 transmission标签

官方介绍见ROS wiki transmissions。transmission标签描述了执行器与关节之间的联系。在七自由度机器人的URDF文件中,使用macro定义了transmission。

1
2
3
4
5
6
7
8
9
10
11
12
<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>

transmission标签包含的元素有:

  • name:指定一个名字。
  • type:目前只有transmission_interface/SimpleTransmission这一种类型。
  • joint:由joint_name指定是哪个关节。
    • hardwareInterface:指定硬件借口类型,这里的接口类型指定了控制器采用什么样的控制方式。在官方解释ROS wiki transmissions中,这样描述:Note that the value of this tag should be EffortJointInterface when this transmission is loaded in Gazebo and hardware_interface/EffortJointInterface when this transmission is loaded in RobotHW.如果有大神看到了,还请给我解释一下。
  • actuator:transmission 连接到的执行器。名字自己取。
    • mechanicalReduction (可选):Specifies a mechanical reduction at the joint/actuator transmission.
    • hardwareInterface (可选):应与joint中的一致。

宏定义后的调用方法为:

1
2
3
4
5
6
7
8
9
<xacro:transmission_block joint_name="shoulder_pan_joint"/>
<xacro:transmission_block joint_name="shoulder_pitch_joint"/>
<xacro:transmission_block joint_name="elbow_roll_joint"/>
<xacro:transmission_block joint_name="elbow_pitch_joint"/>
<xacro:transmission_block joint_name="wrist_roll_joint"/>
<xacro:transmission_block joint_name="wrist_pitch_joint"/>
<xacro:transmission_block joint_name="gripper_roll_joint"/>
<xacro:transmission_block joint_name="finger_joint1"/>
<xacro:transmission_block joint_name="finger_joint2"/>

2.9 include标签

1
<xacro:include filename="$(find mastering_ros_robot_description_pkg)/urdf/sensors/xtion_pro_live.urdf.xacro"/>

使用include标签可以包含其他的URDF文件。

2.10 plugin

在添加transmission标签后,需要添加 gazebo_ros_control plugin 才能将 hardware interfacescontroller manager 联系起来。

1
2
3
4
5
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/seven_dof_arm</robotNamespace>
</plugin>
</gazebo>

在这里,<plugin>...</plugin>元素指定了加载的plugin是 libgazebo_ros_control.so<robotNamespace>...</robotNamespace>可以指定为机器人的名字,如果不指定,会自动加载URDF文件中的 robot name。

参考文献

  1. ROS wiki URDF-XML
  2. ROS与GAZEBO实时硬件仿真(1)——urdf的编写
  3. ROS中URDF模型的joint标签详解
  4. Tutorial: Using a URDF in Gazebo

附录:seven_dof_arm.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
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
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
<?xml version="1.0"?>
<robot name="seven_dof_arm"
xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Include materials -->

<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>

<material name="Red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>

<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>

<!-- Constants -->
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<xacro:property name="M_SCALE" value="0.001 0.001 0.001"/>
<xacro:property name="M_PI" value="3.14159"/>

<!-- Shoulder pan link properties -->
<xacro:property name="shoulder_pan_width" value="0.04" />
<xacro:property name="shoulder_pan_len" value="0.08" />


<!-- Shoulder pitch link properties -->
<xacro:property name="shoulder_pitch_len" value="0.14" />
<xacro:property name="shoulder_pitch_width" value="0.04" />
<xacro:property name="shoulder_pitch_height" value="0.04" />


<!-- Elbow roll link properties -->
<xacro:property name="elbow_roll_width" value="0.02" />
<xacro:property name="elbow_roll_len" value="0.06" />


<!-- Elbow pitch link properties -->
<xacro:property name="elbow_pitch_len" value="0.22" />
<xacro:property name="elbow_pitch_width" value="0.04" />
<xacro:property name="elbow_pitch_height" value="0.04" />


<!-- Wrist roll link properties -->
<xacro:property name="wrist_roll_width" value="0.02" />
<xacro:property name="wrist_roll_len" value="0.04" />


<!-- wrist pitch link properties -->
<xacro:property name="wrist_pitch_len" value="0.06" />
<xacro:property name="wrist_pitch_width" value="0.04" />
<xacro:property name="wrist_pitch_height" value="0.04" />


<!-- Gripper roll link properties -->
<xacro:property name="gripper_roll_width" value="0.04" />
<xacro:property name="gripper_roll_len" value="0.02" />


<!-- Left gripper -->
<xacro:property name="left_gripper_len" value="0.08" />
<xacro:property name="left_gripper_width" value="0.01" />
<xacro:property name="left_gripper_height" value="0.01" />


<!-- Right gripper -->
<xacro:property name="right_gripper_len" value="0.08" />
<xacro:property name="right_gripper_width" value="0.01" />
<xacro:property name="right_gripper_height" value="0.01" />



<!-- Right gripper -->
<xacro:property name="grasp_frame_len" value="0.02" />
<xacro:property name="grasp_frame_width" value="0.02" />
<xacro:property name="grasp_frame_height" value="0.02" />


<xacro:macro name="inertial_matrix" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>

<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>


<!-- BOTTOM FIXED LINK
This link is the base of the arm in which arm is placed

-->

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->


<link name="bottom_link">

<visual>
<origin xyz=" 0 0 -0.04" rpy="0 0 0"/>
<geometry>

<box size="1 1 0.02" />

</geometry>
<material name="Brown" />
</visual>

<collision>
<origin xyz=" 0 0 -0.04" rpy="0 0 0"/>
<geometry>
<box size="1 1 0.02" />
</geometry>
</collision>>

</link>

<gazebo reference="bottom_link">
<material>Gazebo/White</material>
</gazebo>

<joint name="bottom_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_link"/>
<child link="bottom_link"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- BASE LINK -->
<link name="base_link">

<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<!-- rotate PI/2 -->
<geometry>

<box size="0.1 0.1 0.1" />

</geometry>
<material name="White" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<!-- rotate PI/2 -->
<geometry>

<box size="0.1 0.1 0.1" />

</geometry>
</collision>>
<xacro:inertial_matrix mass="1"/>

</link>

<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>

<joint name="shoulder_pan_joint" type="revolute">
<parent link="base_link"/>
<child link="shoulder_pan_link"/>
<origin xyz="0 0 0.05" rpy="0 ${M_PI/2} ${M_PI*0}" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.61799387799" upper="1.98394848567"/>
<dynamics damping="50" friction="1"/>
</joint>


<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- SHOULDER PAN LINK -->
<link name="shoulder_pan_link">

<visual>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>

<cylinder radius="${shoulder_pan_width}" length="${shoulder_pan_len}"/>

</geometry>
<material name="Red" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${shoulder_pan_width}" length="${shoulder_pan_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="shoulder_pan_link">
<material>Gazebo/Red</material>
</gazebo>

<joint name="shoulder_pitch_joint" type="revolute">
<parent link="shoulder_pan_link"/>
<child link="shoulder_pitch_link"/>
<origin xyz="-0.041 0.0021 0.0" rpy="-${M_PI/2} 0 ${M_PI/2}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-1.19962513147" upper="1.89994105047" />
<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->
<!-- SHOULDER PITCH LINK -->
<link name="shoulder_pitch_link">

<visual>
<origin xyz="-0.002 0 0.04" rpy="0 ${M_PI/2} 0" />
<geometry>

<box size="${shoulder_pitch_len} ${shoulder_pitch_width} ${shoulder_pitch_height}" />

</geometry>
<material name="White" />
</visual>

<collision>
<origin xyz="-0.002 0 0.04" rpy="0 ${M_PI/2} 0" />
<geometry>
<box size="${shoulder_pitch_len} ${shoulder_pitch_width} ${shoulder_pitch_height}" />
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="shoulder_pitch_link">
<material>Gazebo/White</material>
</gazebo>

<joint name="elbow_roll_joint" type="revolute">
<parent link="shoulder_pitch_link"/>
<child link="elbow_roll_link"/>
<origin xyz="-0.002 0 0.1206" rpy="${M_PI} ${M_PI/2} 0" />
<axis xyz="-1 0 0" />
<limit effort="300" velocity="1" lower="-2.61799387799" upper="0.705631162427" />
<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->
<!-- ELBOW ROLL LINK -->
<link name="elbow_roll_link">

<visual>
<origin xyz="-0.015 0.0 -0.0" rpy="0 ${M_PI/2} 0" />
<geometry>

<cylinder radius="${elbow_roll_width}" length="${elbow_roll_len}"/>

</geometry>
<material name="Black" />
</visual>

<collision>
<origin xyz="-0.015 0.0 -0.0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${elbow_roll_width}" length="${elbow_roll_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="elbow_roll_link">
<material>Gazebo/Black</material>
</gazebo>

<joint name="elbow_pitch_joint" type="revolute">
<parent link="elbow_roll_link"/>
<child link="elbow_pitch_link"/>
<origin xyz="-0.035 0 0.0" rpy="0.055 ${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-1.5953400194" upper="1.93281579274" />
<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- ELBOW PITCH LINK -->
<link name="elbow_pitch_link">

<visual>
<origin xyz="0 0 -0.12" rpy="0 ${M_PI/2} 0" />
<geometry>

<box size="${elbow_pitch_len} ${elbow_pitch_width} ${elbow_pitch_height}" />

</geometry>
<material name="Red" />
</visual>

<collision>
<origin xyz="0 0 -0.12" rpy="0 ${M_PI/2} 0" />
<geometry>
<box size="${elbow_pitch_len} ${elbow_pitch_width} ${elbow_pitch_height}" />
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="elbow_pitch_link">
<material>Gazebo/Red</material>
</gazebo>

<joint name="wrist_roll_joint" type="revolute">
<parent link="elbow_pitch_link"/>
<child link="wrist_roll_link"/>
<origin xyz="0.0 0.0081 -.248" rpy="0 ${M_PI/2} ${M_PI}" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-2.61799387799" upper="2.6128806087" />
<dynamics damping="50" friction="1"/>
</joint>

<!-- WRIST ROLL LINK -->
<link name="wrist_roll_link">
<visual>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>

<cylinder radius="${elbow_roll_width}" length="${elbow_roll_len}"/>

</geometry>
<material name="Black" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${elbow_roll_width}" length="${elbow_roll_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>

</link>

<gazebo reference="wrist_roll_link">
<material>Gazebo/Black</material>
</gazebo>

<joint name="wrist_pitch_joint" type="revolute">
<parent link="wrist_roll_link"/>
<child link="wrist_pitch_link"/>
<origin xyz="0.0 0.0 0.0001" rpy="0 ${M_PI/2} 0" />
<axis xyz="1 0 0" />
<limit effort="300" velocity="1" lower="-1.5953400194" upper="1.98394848567" />
<dynamics damping="50" friction="1"/>
</joint>


<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->
<!-- WRIST PITCH LINK -->
<link name="wrist_pitch_link">

<visual>
<origin xyz="0 0 0.04" rpy="0 ${M_PI/2} 0" />
<geometry>

<box size="${wrist_pitch_len} ${wrist_pitch_width} ${wrist_pitch_height}" />

</geometry>
<material name="White" />
</visual>

<collision>
<origin xyz="0 0 0.04 " rpy="0 ${M_PI/2} 0" />
<geometry>
<box size="${wrist_pitch_len} ${wrist_pitch_width} ${wrist_pitch_height}" />
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>

</link>

<gazebo reference="wrist_pitch_link">
<material>Gazebo/White</material>
</gazebo>

<joint name="gripper_roll_joint" type="revolute">
<parent link="wrist_pitch_link"/>
<child link="gripper_roll_link"/>
<origin xyz="0 0 0.080" rpy="${1.5*M_PI} -${.5*M_PI} 0" />
<axis xyz="1 0 0" />
<!--
<limit effort="300" velocity="1" lower="-2.61799387799" upper="2.6128806087" />
-->
<limit effort="300" velocity="1" lower="-3.14" upper="3.14" />
<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- GRIPPER ROLL LINK -->
<link name="gripper_roll_link">

<visual>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>

<cylinder radius="${gripper_roll_width}" length="${gripper_roll_len}"/>

</geometry>
<material name="Red" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
<geometry>
<cylinder radius="${gripper_roll_width}" length="${gripper_roll_len}"/>
</geometry>
</collision>
<xacro:inertial_matrix mass="1"/>

</link>

<gazebo reference="gripper_roll_link">
<material>Gazebo/Red</material>
</gazebo>


<joint name="finger_joint1" type="prismatic">
<parent link="gripper_roll_link"/>
<child link="gripper_finger_link1"/>
<origin xyz="0.0 0 0" />
<axis xyz="0 1 0" />
<limit effort="100" lower="0" upper="0.03" velocity="1.0"/>

<safety_controller k_position="20" k_velocity="20" soft_lower_limit="${-0.15 }" soft_upper_limit="${ 0.0 }"/>

<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- LEFT GRIPPER AFT LINK -->
<link name="gripper_finger_link1">

<visual>
<origin xyz="0.04 -0.03 0"/>
<geometry>

<box size="${left_gripper_len} ${left_gripper_width} ${left_gripper_height}" />

</geometry>
<material name="White" />
</visual>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="l_gripper_aft_link">
<material>Gazebo/White</material>
</gazebo>

<!-- Joint between Wrist roll and finger 2 -->

<joint name="finger_joint2" type="prismatic">
<parent link="gripper_roll_link"/>
<child link="gripper_finger_link2"/>
<origin xyz="0.0 0 0" />
<axis xyz="0 1 0" />
<limit effort="1" lower="-0.03" upper="0" velocity="1.0"/>

<!-- <mimic joint="gripper_finger_joint" multiplier="-1.0" offset="0.0" /> -->

<dynamics damping="50" friction="1"/>
</joint>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->

<!-- RIGHT GRIPPER AFT LINK -->


<link name="gripper_finger_link2">
<visual>
<origin xyz="0.04 0.03 0"/>
<geometry>
<box size="${right_gripper_len} ${right_gripper_width} ${right_gripper_height}" />
</geometry>
<material name="White" />
</visual>
<xacro:inertial_matrix mass="1"/>
</link>

<gazebo reference="r_gripper_aft_link">
<material>Gazebo/White</material>
</gazebo>



<!-- Grasping frame -->
<link name="grasping_frame">

<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.0001"/>
<cuboid_inertia mass="0.0001" x="0.001" y="0.001" z="0.001"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />

</inertial>

<visual>
<origin xyz="0 0 0"/>
<geometry>

<box size="${grasp_frame_len} ${grasp_frame_width} ${grasp_frame_height}" />

</geometry>
<material name="White" />
</visual>

</link>

<joint name="grasping_frame_joint" type="fixed">
<parent link="gripper_roll_link"/>
<child link="grasping_frame"/>
<origin xyz="0.08 0 0" rpy="0 0 0"/>
</joint>

<xacro:include filename="$(find mastering_ros_robot_description_pkg)/urdf/sensors/xtion_pro_live.urdf.xacro"/>

<!-- ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -->
<!-- Transmissions for ROS Control -->

<xacro:transmission_block joint_name="shoulder_pan_joint"/>
<xacro:transmission_block joint_name="shoulder_pitch_joint"/>
<xacro:transmission_block joint_name="elbow_roll_joint"/>
<xacro:transmission_block joint_name="elbow_pitch_joint"/>
<xacro:transmission_block joint_name="wrist_roll_joint"/>
<xacro:transmission_block joint_name="wrist_pitch_joint"/>
<xacro:transmission_block joint_name="gripper_roll_joint"/>
<xacro:transmission_block joint_name="finger_joint1"/>
<xacro:transmission_block joint_name="finger_joint2"/>

<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/seven_dof_arm</robotNamespace>
</plugin>
</gazebo>

</robotNamespace>
------ 本文结束感谢您的阅读------
Donate a cup of cola?