一、URDF集成Rviz基本流程
URDF 不能单独使用,需要结合 Rviz 或 Gazebo,URDF 只是一个文件,需要在 Rviz 或 Gazebo 中渲染成图形化的机器人模型,当前,首先演示URDF与Rviz的集成使用,因为URDF与Rviz的集成较之于URDF与Gazebo的集成更为简单,后期,基于Rviz的集成实现,我们再进一步介绍URDF语法。
需求描述:
在 Rviz 中显示一个盒状机器人
实现流程:
- 准备:新建功能包,导入依赖
- 核心:编写 urdf 文件
- 核心:在 launch 文件集成 URDF 与 Rviz
- 在 Rviz 中显示机器人模型创建功能包,导入依赖
1、创建功能包,导入依赖
创建一个新的功能包,名称自定义,导入依赖包:urdf
与xacro
在当前功能包下,再新建几个文件夹:
urdf
: 存储 urdf 文件的目录
meshes
:机器人模型渲染文件(暂不使用),给皮肤用的
config
: 配置文件
launch
: 存储 launch 启动文件
2、编写URDF文件
新建一个自己文件夹urdf,新建urdf文件:demo01_helloworld.urdf
<robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
</visual>
</link>
</robot>
3、在 launch 文件中集成 URDF 与 Rviz
新建launch文件:demo01_helloworld.launch
<launch>
<!--
1、在参数服务器载入urdf文件
2、启动rviz,并将当前配置保存进config目录
-->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo01_helloworld.urdf" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
</launch>
4、在 Rviz 中显示机器人模型
启动launch文件之后,启动rviz文件
二、URDF语法详解
URDF 文件是一个标准的 XML 文件,在 ROS 中预定义了一系列的标签用于描述机器人模型,机器人模型可能较为复杂,但是 ROS 的 URDF 中机器人的组成却是较为简单,可以主要简化为两部分:连杆(link标签) 与 关节(joint标签),接下来我们就通过案例了解一下 URDF 中的不同标签:
- robot 根标签,类似于 launch文件中的launch标签
- link 连杆标签
- joint 关节标签
- gazebo 集成gazebo需要使用的标签
关于gazebo标签,后期在使用 gazebo 仿真时,才需要使用到,用于配置仿真环境所需参数,比如: 机器人材料属性、gazebo插件等,但是该标签不是机器人模型必须的,只有在仿真时才需设置
1、robot
urdf 中为了保证 xml 语法的完整性,使用了robot
标签作为根标签,所有的 link 和 joint 以及其他标签都必须包含在 robot 标签内,在该标签内可以通过 name 属性设置机器人模型的名称
相当于launch标签
(1)属性
name: 指定机器人模型的名称
(2)子标签
其他标签都是子级标签
2、link
urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性,比如: 机器人底座、轮子、激光雷达、摄像头...每一个部件都对应一个 link, 在 link 标签内,可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性
(1)属性
- name ---> 为连杆命名
(2)子标签
- visual ---> 描述外观(对应的数据是可视的)
- geometry 设置连杆的形状
- 标签1: box(盒状)
- 属性:size=长(x) 宽(y) 高(z)
- 标签2: cylinder(圆柱)
- 属性:radius=半径 length=高度
- 标签3: sphere(球体)
- 属性:radius=半径
- 标签4: mesh(为连杆添加皮肤)
- 属性: filename=资源路径(格式:package://<packagename>/<path>/文件)
- origin 设置偏移量与倾斜弧度
- 属性1: xyz=x偏移 y便宜 z偏移
- 属性2: rpy=x翻滚 y俯仰 z偏航 (单位是弧度)
- metrial 设置材料属性(颜色)
- 属性: name
- 标签: color
- 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
- collision ---> 连杆的碰撞属性
- Inertial ---> 连杆的惯性矩阵
(3)实例
<!--设置不同形状的机器人部件-->
<robot name="mycar">
<link name="base_link">
<!--可视化标签-->
<visual>
<!--1、形状-->
<geometry>
<!--立方体-->
<!-- <box size="0.3 0.2 0.1"/> -->
<!--圆柱体-->
<!-- <cylinder radius="0.1" length="2"/> -->
<!--球体-->
<!-- <sphere radius="0.5"/> -->
<!--皮肤-->
<mesh filename="package://urdf01_rviz/meshes/autolabor_mini.stl"/>
</geometry>
<!--2、偏移量和弧度-->
<!--
xyz设置机器人模型在xyz上的偏移量
rpy设置倾斜弧度 x:翻滚 y:俯仰 z:偏航
-->
<origin xyz="0 0 0" rpy="1.57 0 1.57"/>
<!--3、颜色-->
<!--
rgba: r=red g=green b=blue a=透明度
都在0-1之间
-->
<material name="car_color" >
<color rgba="0 0 1 1" />
</material>
</visual>
</link>
</robot>
备注:xml注释是 ctrl+?
3、joint
urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性,还可以指定关节运动的安全极限,机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接,不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制....,比如:安装在底座上的轮子可以360度旋转,而摄像头则可能是完全固定在底座上。
joint标签对应的数据在模型中是不可见的
(1)属性
- name ---> 为关节命名
- type ---> 关节运动形式
- continuous: 旋转关节,可以绕单轴无限旋转
- revolute: 旋转关节,类似于 continues,但是有旋转角度限制
- prismatic: 滑动关节,沿某一轴线移动的关节,有位置极限
- planer: 平面关节,允许在平面正交方向上平移或旋转
- floating: 浮动关节,允许进行平移、旋转运动
- fixed: 固定关节,不允许运动的特殊关节
(2)子标签
- parent(必需的)
parent link的名字是一个强制的属性: - link:父级连杆的名字,是这个link在机器人结构树中的名字。
- child(必需的)
child link的名字是一个强制的属性: - link:子级连杆的名字,是这个link在机器人结构树中的名字。
- origin
- 属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
- axis
- 属性: xyz用于设置围绕哪个关节轴运动。
- collision ---> 连杆的碰撞属性
- Inertial ---> 连杆的惯性矩阵
(3)案例
需求:创建机器人模型,底盘为长方体,在长方体的前面添加一摄像头,摄像头可以沿着 Z 轴 360 度旋转。
新建urdf文件:demo03_joint.urdf
<robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="blue">
<color rgba="0.8 0.5 0 0.5" />
</material>
</visual>
</link>
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<origin xyz="0 0 0.025" rpy="0 0 0" />
<material name="red">
<color rgba="0 0 1 0.5" />
</material>
</visual>
</link>
<joint name="camera2base" type="continuous">
<parent link="base_link"/>
<child link="camera" />
<origin xyz="0.12 0 0.05" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
</robot>
新建launch文件:demo03_join.launch
<launch>
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo03_joint.urdf" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
<!-- 添加关节状态发布节点 -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 添加机器人状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- 可选:用于控制关节运动的节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>
运行launch文件
(4)可能出现的错误
问题1:
命令行输出如下错误提示
UnicodeEncodeError: 'ascii' codec can't encode characters in position 463-464: ordinal not in range(128)
[joint_state_publisher-3] process has died [pid 4443, exit code 1, cmd /opt/ros/melodic/lib/joint_state_publisher/joint_state_publisher __name:=joint_state_publisher __log:=/home/rosmelodic/.ros/log/b38967c0-0acb-11eb-aee3-0800278ee10c/joint_state_publisher-3.log].
log file: /home/rosmelodic/.ros/log/b38967c0-0acb-11eb-aee3-0800278ee10c/joint_state_publisher-3*.log
rviz中提示坐标变换异常,导致机器人部件显示结构异常
原因:编码问题导致的
解决:去除URDF中的中文注释
问题2:[ERROR] [1584370263.037038]: Could not find the GUI, install the 'joint_state_publisher_gui' package
解决:sudo apt install ros-noetic-joint-state-publisher-gui
(5)base_footprint优化urdf
<robot name="mycar">
<link name="base_footprint">
<visual>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="blue">
<color rgba="0.8 0.5 0 0.5" />
</material>
</visual>
</link>
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<origin xyz="0 0 0.025" rpy="0 0 0" />
<material name="red">
<color rgba="0 0 1 0.5" />
</material>
</visual>
</link>
<joint name="link2footprint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link" />
<origin xyz="0 0 0.05" rpy="0 0 0" />
</joint>
<joint name="camera2base" type="continuous">
<parent link="base_link"/>
<child link="camera" />
<origin xyz="0.12 0 0.05" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
三、URDF练习
1、需求描述
创建一个四轮圆柱状机器人模型,机器人参数如下,底盘为圆柱状,半径 10cm,高 8cm,四轮由两个驱动轮和两个万向支撑轮组成,两个驱动轮半径为 3.25cm,轮胎宽度1.5cm,两个万向轮为球状,半径 0.75cm,底盘离地间距为 1.5cm(与万向轮直径一致)
2、新建urdf以及launch文件
新建urdf文件:
<robot name="mycar">
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001"/>
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="0.1" length="0.08"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="yellow">
<color rgba="0.8 0.3 0.1 0.5" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 0.055"/>
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5705 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="left_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="left_wheel" />
<origin xyz="0 0.1 -0.0225" />
<axis xyz="0 1 0" />
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5705 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="right_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="right_wheel" />
<origin xyz="0 -0.1 -0.0225" />
<axis xyz="0 1 0" />
</joint>
<link name="front_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="front_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="front_wheel" />
<origin xyz="0.0925 0 -0.0475" />
<axis xyz="1 1 1" />
</joint>
<link name="back_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="back_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="back_wheel" />
<origin xyz="-0.0925 0 -0.0475" />
<axis xyz="1 1 1" />
</joint>
</robot>
新建launch文件:
<launch>
<!--将urdf文件内容设置进参数服务器-->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo05_test.urdf" />
<!--启动rviz-->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
<!--启动机器人状态和关节状态发布节点-->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
<!--启动图形化的控制关节运动节点-->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui"/>
</launch>
运行:
四、URDF工具
1、check_urdf 语法检查
进入urdf文件所属目录,调用:check_urdf urdf文件
,如果不抛出异常,说明文件合法,否则非法
2、urdf_to_graphiz 结构查看
进入urdf文件所属目录,调用:urdf_to_graphiz urdf文件
,当前目录下会生成 pdf 文件
参考:
评论(0)
您还未登录,请登录后发表或查看评论