一、URDF文件简介
URDF(Unified Robot Description Format),统一机器人描述格式,是一种特殊的xml格式,用来描述一个机器人. 在ROS中,urdf功能包包含一个urdf格式文件的C++解析器,这样,任何通过统一编码格式设计的机器人都可以通过该解析器得到一个可视化的模型。
URDF创造的机器人模型包含的内容有:
- 连杆 link
- 关节 joint
- 运动学参数 axis
- 动力学参数 dynamics
- 可视化模型 visual
- 碰撞检测模型 collision
在ROS中,常常有以xxx_description命名的包,这些包中包含的文件就是某个机器人的描述文件,如著名的turtlebot_description等。
这里着重描述下joint:
joint就是用来连接两个具体的link部分的:
- 描述机器人关节的运动学和动力学属性
- 包括关节运动的位置和速度限制
- 根据关节的运动形式,可以将其分为六种类型
关节类型 描述 continuous 旋转关节,可以围绕单轴无限旋转 revolute 旋转关节,类似于 continuous,但是有旋转的角度极限 prismatic 滑动关节,沿某一轴线移动的关节,带有位置极限 planar 平面关节,允许在平面正交方向上平移或旋转 floating 浮动关节,允许进行平移、旋转运动 fixed 固定关节,不允许运动的特殊关节
<joint name="joint6" type="revolute">
<origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
<parent link="Link5" />
<child link="Link6" />
<axis xyz="0 0 1" />
<limit lower="-6.283" upper="6.283" effort="100" velocity="1" />
</joint>
joint是连接两个link,需要分一个主次关系,主关节是parent link,子关节是child link, 在xml形式的描述中这个两个 link是必须存在的。在上述”joint6”中,可以看到parent link是Link5(即关节5的link),child link是Link6(即关节6的link)。在< limit >中:描述该关节运动的一些极限值,包括关节运动的上下限位置、速度限制、力矩限制等。
本文章涉及的示例来自于睿尔曼机器人,如需获取最新资料可联系睿尔曼技术支持团队
二、模型整合
本文中用到的URDF有:RM65-B、两指夹持器和夹持器固定板。
1、夹持器固定板STL文件放置
将夹持器的固定板STL文件放到机械臂RM65-B的rm_65_b_description/meshes下,如下图所示:
2、夹持器固定板URDF文件放置
将夹持器固定板URDF文件中的link中的内容复制到rm_65_b_description.urdf文件中,需注意复制的内容需格式完整。
蓝色框内为夹持器固定板URDF文件,红色框内为STL的文件路径,我们需将其更改为第一步放置STL文件的路径即可。(例:rm_65_b_description/meshes)否则会出现无法打开该STL文件的错误。
接下来我们打开终端运行:
roslaunch rm_65_b_description display.launch
可以看到会出现以下错误:
这是因为夹持器固定板URDF文件的link和机械臂的base_link产生冲突,不能同时在rviz中打开.所以我们需将其变更为机械臂的一个joint,这样就会使得夹持器固定板的link与机械臂的base_link产生关联关系。
3、添加joint
这里添加了joint,并将其类型更改为fixed,使夹持器固定板的fixed_plate_link与机械臂link6产生父子关联。
接下来我们打开终端运行:
roslaunch rm_65_b_description display.launch
成功运行后可看到以下内容:
夹持器固定板成功被固定在机械臂上,但未与机械臂末端法兰盘完全接触,接下来我们需点击rviz左下角的Add添加tf,勾选link6,其中会出现link6的坐标系,红绿蓝分别为x、y、z轴。可根据夹持器固定板距离机械臂末端法兰盘的高度来不断的修改joint "fixed_plate"中 <origin xyz="0 0 0.007" rpy="3.1415 0 0" /> z轴的值,最终使得夹持器固定板与机械臂末端法兰盘刚好完全接触。
4、夹持器STL文件放置
将夹持器STL文件文件放到机械臂RM65-B的rm_65_b_description/meshes下,如下图所示:
5、夹持器URDF文件放置
将夹持器URDF文件中的link中的内容复制到rm_65_b_description.urdf文件中,需注意复制的内容需格式完整。
复制后仍需更改夹持器STL文件的路径即可。(例:rm_65_b_description/meshes)否则会出现无法打开该STL文件的错误。
6、添加joint
这里添加了joint,并将其类型更改为fixed,使夹持器的gripper_base_link与夹持器固定板的fixed_plate_link产生父子关联。如下图:
打开rviz可以看到
接下来我们需点击rviz左下角的Add添加tf,勾选fixed_plate_link,其中会出现fixed_plate_link的坐标系,红绿蓝分别为x、y、z轴。可根据夹持器距离夹持器固定板的高度来不断的修改joint "gripper_base_link"中 <origin xyz="0.011 0.0 -0.042" rpy="0.0 1.5708 0.0" /> x y z轴的值,更改rx ry rz的值可更改夹持器的姿态,最终使得夹持器与夹持器固定板刚好完全接触。
最终效果:
7、总结
关于在机械臂urdf中添加夹持器和夹持器固定板的urdf,建议是通过在solidworks构建好模型后再导出URDF文件,否则会出现些许安装误差。
8、完整URDF
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="rm_65_b_description">
<link
name="base_link">
<inertial>
<origin
xyz="-0.00043233 -3.2765E-05 0.059942"
rpy="0 0 0" />
<mass
value="0.84108" />
<inertia
ixx="0.0017261"
ixy="2.4864E-06"
ixz="-3.6752E-05"
iyy="0.0017099"
iyz="1.7199E-06"
izz="0.00090404" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="Link1">
<inertial>
<origin
xyz="1.4691E-07 0.021109 -0.025186"
rpy="0 0 0" />
<mass
value="0.59356" />
<inertia
ixx="0.0012661"
ixy="-1.2313E-08"
ixz="-1.0057E-08"
iyy="0.0011817"
iyz="-0.00021122"
izz="0.00056135" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link1.STL" />
</geometry>
</collision>
</link>
<joint
name="joint1"
type="revolute">
<origin
xyz="0 0 0.2405"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="Link1" />
<axis
xyz="0 0 1" />
<limit
lower="-3.107"
upper="3.107"
effort="100"
velocity="1" />
</joint>
<link
name="Link2">
<inertial>
<origin
xyz="0.15226 4.2545E-07 -0.0062026"
rpy="0 0 0" />
<mass
value="0.86418" />
<inertia
ixx="0.00063253"
ixy="-8.795E-09"
ixz="0.00042163"
iyy="0.0020527"
iyz="2.3725E-09"
izz="0.0019528" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link2.STL" />
</geometry>
</collision>
</link>
<joint
name="joint2"
type="revolute">
<origin
xyz="0 0 0"
rpy="1.5708 -1.5708 0" />
<parent
link="Link1" />
<child
link="Link2" />
<axis
xyz="0 0 1" />
<limit
lower="-2.269"
upper="2.269"
effort="100"
velocity="1" />
</joint>
<link
name="Link3">
<inertial>
<origin
xyz="4.7938E-06 -0.059593 0.010569"
rpy="0 0 0" />
<mass
value="0.28965" />
<inertia
ixx="0.00063737"
ixy="-5.9726E-08"
ixz="-3.3299E-08"
iyy="0.00015649"
iyz="-0.00014461"
izz="0.00061418" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link3.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link3.STL" />
</geometry>
</collision>
</link>
<joint
name="joint3"
type="revolute">
<origin
xyz="0.256 0 0"
rpy="0 0 1.5708" />
<parent
link="Link2" />
<child
link="Link3" />
<axis
xyz="0 0 1" />
<limit
lower="-2.356"
upper="2.356"
effort="100"
velocity="1" />
</joint>
<link
name="Link4">
<inertial>
<origin
xyz="1.0293E-06 -0.018042 -0.021539"
rpy="0 0 0" />
<mass
value="0.23942" />
<inertia
ixx="0.00028594"
ixy="2.854E-09"
ixz="-1.9592E-09"
iyy="0.00026273"
iyz="4.4237E-05"
izz="0.00011989" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link4.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link4.STL" />
</geometry>
</collision>
</link>
<joint
name="joint4"
type="revolute">
<origin
xyz="0 -0.21 0"
rpy="1.5708 0 0" />
<parent
link="Link3" />
<child
link="Link4" />
<axis
xyz="0 0 1" />
<limit
lower="-3.107"
upper="3.107"
effort="100"
velocity="1" />
</joint>
<link
name="Link5">
<inertial>
<origin
xyz="3.4528E-06 -0.059382 0.0073678"
rpy="0 0 0" />
<mass
value="0.21879" />
<inertia
ixx="0.00035053"
ixy="-3.165E-08"
ixz="-1.7434E-08"
iyy="0.00010492"
iyz="-7.824E-05"
izz="0.00033447" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link5.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link5.STL" />
</geometry>
</collision>
</link>
<joint
name="joint5"
type="revolute">
<origin
xyz="0 0 0"
rpy="-1.5708 0 0" />
<parent
link="Link4" />
<child
link="Link5" />
<axis
xyz="0 0 1" />
<limit
lower="-2.234"
upper="2.234"
effort="100"
velocity="1" />
</joint>
<link
name="Link6">
<inertial>
<origin
xyz="0.00081643 1.3288E-05 -0.012705"
rpy="0 0 0" />
<mass
value="0.065035" />
<inertia
ixx="2.1143E-05"
ixy="-2.2878E-08"
ixz="-2.5601E-08"
iyy="1.811E-05"
iyz="-1.0178E-08"
izz="3.19E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link6.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/Link6.STL" />
</geometry>
</collision>
</link>
<joint
name="joint6"
type="revolute">
<origin
xyz="0 -0.144 0"
rpy="1.5708 0 0" />
<parent
link="Link5" />
<child
link="Link6" />
<axis
xyz="0 0 1" />
<limit
lower="-6.283"
upper="6.283"
effort="100"
velocity="1" />
</joint>
<!--转接板-->
<link
name="fixed_plate_link">
<inertial>
<origin
xyz="0.045819 6.35930000024774E-09 0.0040461"
rpy="0 0 0" />
<mass
value="0.13853" />
<inertia
ixx="4.2971E-05"
ixy="4.08909999999764E-11"
ixz="3.8593E-07"
iyy="0.00023364"
iyz="1.36420000025386E-11"
izz="0.0002751" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="1.5707963267949 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/fixed_plate.STL" />
</geometry>
<material
name="">
<color
rgba="0.89804 0.91765 0.92941 1" />
<texture
filename="package://rm_65_b_description/textures/" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="1.5707963267949 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/fixed_plate.STL" />
</geometry>
</collision>
</link>
<joint
name="fixed_plate"
type="fixed">
<origin
xyz="0 0 0.007"
rpy="3.1415 0 0" />
<parent
link="Link6" />
<child
link="fixed_plate_link" />
<axis
xyz="0 0 1" />
</joint>
<!--两指夹持器-->
<link
name="gripper_base_link">
<inertial>
<origin
xyz="0.00339303654278191 -2.1205433560656E-05 -0.0099185365078758"
rpy="0 0 0" />
<mass
value="0.105853592307918" />
<inertia
ixx="3.93795927851199E-05"
ixy="-8.7395226509728E-08"
ixz="5.91928672040786E-07"
iyy="0.000106376720416166"
iyz="-1.82858764303774E-10"
izz="0.000125852616063213" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.894117647058823 0.913725490196078 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_base_link.STL" />
</geometry>
</collision>
</link>
<joint
name="gripper_base_link"
type="fixed">
<origin
xyz="0.011 0.0 -0.042"
rpy="0.0 1.5708 0.0" />
<parent
link="fixed_plate_link" />
<child
link="gripper_base_link" />
<axis
xyz="0 0 1" />
</joint>
<link
name="gripper_link1">
<inertial>
<origin
xyz="0.0164149658242926 9.75754915755189E-09 0.00898193900447147"
rpy="0 0 0" />
<mass
value="0.00647716099397792" />
<inertia
ixx="2.95068972140683E-07"
ixy="8.63208170411155E-13"
ixz="-1.92008628754607E-09"
iyy="1.69979619476872E-06"
iyz="-1.99232397395103E-13"
izz="1.45016449434989E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link1.STL" />
</geometry>
<material
name="">
<color
rgba="0.894117647058823 0.913725490196078 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link1.STL" />
</geometry>
</collision>
</link>
<joint
name="gripper_link1"
type="revolute">
<origin
xyz="0.036 -0.04 -0.019"
rpy="-3.0158E-16 -7.1586E-16 -0.30719" />
<parent
link="gripper_base_link" />
<child
link="gripper_link1" />
<axis
xyz="0 0 1" />
<limit
lower="0"
upper="1"
effort="1"
velocity="1" />
</joint>
<link
name="gripper_link11">
<inertial>
<origin
xyz="0.011366555972498 0.0118642725662323 0.0060000073930068"
rpy="0 0 0" />
<mass
value="0.0127543420299361" />
<inertia
ixx="5.68538478020233E-07"
ixy="-2.68012262729007E-07"
ixz="9.34125984931838E-13"
iyy="1.45145776457487E-06"
iyz="9.34355360561326E-13"
izz="1.50564598220449E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link11.STL" />
</geometry>
<material
name="">
<color
rgba="0.894117647058823 0.913725490196078 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link11.STL" />
</geometry>
</collision>
</link>
<joint
name="gripper_link11"
type="revolute">
<origin
xyz="0.042 0 0.003"
rpy="5.0393E-16 5.6225E-16 0.30719" />
<parent
link="gripper_link1" />
<child
link="gripper_link11" />
<axis
xyz="0 0 1" />
<limit
lower="-1"
upper="0"
effort="1"
velocity="1" />
</joint>
<link
name="gripper_link2">
<inertial>
<origin
xyz="0.0164149658242926 9.75754915755189E-09 0.00898193900447147"
rpy="0 0 0" />
<mass
value="0.00647716099397793" />
<inertia
ixx="2.95068972140683E-07"
ixy="8.63208169650113E-13"
ixz="-1.92008628755341E-09"
iyy="1.69979619476872E-06"
iyz="-1.99232397320157E-13"
izz="1.45016449434989E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link2.STL" />
</geometry>
<material
name="">
<color
rgba="0.894117647058823 0.913725490196078 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link2.STL" />
</geometry>
</collision>
</link>
<joint
name="gripper_link2"
type="revolute">
<origin
xyz="0.036 0.04 -0.019"
rpy="-2.6397E-16 3.7199E-16 0.30719" />
<parent
link="gripper_base_link" />
<child
link="gripper_link2" />
<axis
xyz="0 0 1" />
<limit
lower="-1"
upper="0"
effort="1"
velocity="1" />
</joint>
<link
name="gripper_link22">
<inertial>
<origin
xyz="-0.011366555972498 0.0118642725662323 -0.0060000073930068"
rpy="0 0 0" />
<mass
value="0.0127543420299362" />
<inertia
ixx="5.68538478020238E-07"
ixy="2.6801226272901E-07"
ixz="9.34125986250171E-13"
iyy="1.45145776457489E-06"
iyz="-9.34355361631596E-13"
izz="1.5056459822045E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link22.STL" />
</geometry>
<material
name="">
<color
rgba="0.894117647058823 0.913725490196078 0.929411764705882 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rm_65_b_description/meshes/gripper_link22.STL" />
</geometry>
</collision>
</link>
<joint
name="gripper_link22"
type="revolute">
<origin
xyz="0.042 0 0.015"
rpy="-3.641E-16 2.4584E-16 2.8344" />
<parent
link="gripper_link2" />
<child
link="gripper_link22" />
<axis
xyz="0 0 1" />
<limit
lower="0"
upper="1"
effort="1"
velocity="1" />
</joint>
</robot>