进来聊一聊URDF啊~
1. 概述 先放出官方文件镇楼:
ROS wiki urdf Tutorials .
ROS wiki xacro
ROS wiki urdf
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" /> <xacro:property name ="M_SCALE" value ="0.001 0.001 0.001" /> <xacro:property name ="M_PI" value ="3.14159" /> <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 的内容可以分为visual 、collision 、inertial 三大类。这里我们给出 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" /> <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" /> <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 interfaces 和 controller 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。
参考文献
ROS wiki URDF-XML
ROS与GAZEBO实时硬件仿真(1)——urdf的编写
ROS中URDF模型的joint标签详解
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" > <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 > <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" /> <xacro:property name ="shoulder_pan_width" value ="0.04" /> <xacro:property name ="shoulder_pan_len" value ="0.08" /> <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" /> <xacro:property name ="elbow_roll_width" value ="0.02" /> <xacro:property name ="elbow_roll_len" value ="0.06" /> <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" /> <xacro:property name ="wrist_roll_width" value ="0.02" /> <xacro:property name ="wrist_roll_len" value ="0.04" /> <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" /> <xacro:property name ="gripper_roll_width" value ="0.04" /> <xacro:property name ="gripper_roll_len" value ="0.02" /> <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" /> <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" /> <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 > <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 > <link name ="base_link" > <visual > <origin xyz ="0 0 0" rpy ="${M_PI/2} 0 0" /> <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" /> <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 > <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 > <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 > <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 > <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 > <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 > <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 ="-3.14" upper ="3.14" /> <dynamics damping ="50" friction ="1" /> </joint > <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 > <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 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" /> <dynamics damping ="50" friction ="1" /> </joint > <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 > <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" /> <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" /> <gazebo > <plugin name ="gazebo_ros_control" filename ="libgazebo_ros_control.so" > <robotNamespace > /seven_dof_arm</robotNamespace > </plugin > </gazebo > </robotNamespace >