【Gazebo入门教程】第三讲 SDF文件的静/动态编程建模
文章目录
- 【Gazebo入门教程】第三讲 SDF文件的静/动态编程建模
- 一、自定义模型并导入Gazebo
- 1. 基础操作准备
- 2. 建立模型基础部件(静态)
- 3. 创建关节连接部件(动态)
- 4. Gazebo基本仿真
- 二、创建Velodyne HDL-32 LiDAR传感器
- 1. 创建基本世界
- 2. 创建传感器静态模型
- 3. 添加模型惯性
- 4. 添加关节
- 5. 添加传感器
- 总结
一、自定义模型并导入Gazebo
- 内容简介:本节内容以一个两轮移动机器人为例,使用差动驱动机构运动,从无到有,使用SDF完成建模并在Gazebo中完成仿真,重点在于通过建模的细致流程掌握如何使用SDF文件和Gazebo软件完成机器人仿真。
1. 基础操作准备
- 注意:Gazebo的模型文件有着严格的要求,具体规则可见SDF格式
\qquad
① 创建模型目录
mkdir -p ~/.gazebo/models/my_robot
\qquad
② 创建模型配置文件
gedit ~/.gazebo/models/my_robot/model.config
\qquad
元数据(config)内容如下:
<?xml version="1.0"?>
<model>
<name>My Robot</name>
<version>1.0</version>
<sdf version='1.4'>model.sdf</sdf>
<author>
<name>My Name</name>
<email>me@my.email</email>
</author>
<description>
My awesome robot.
</description>
</model>
\qquad
② 创建模型配置文件
gedit ~/.gazebo/models/my_robot/model.sdf
\qquad
必要标记(sdf)内容如下:
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="my_robot">
</model>
</sdf>
2. 建立模型基础部件(静态)
- 基本要求:本节内容主要是单独创建机器人的各部件,例如底座、轮子等,不涉及相关关节连杆等链接内容,重点在于对齐组件,故模型为静态,忽略物理效果。
\qquad
① 令机器人模型静态
注意:在static标签后将会在link标签下生成对应部件,通过collision的name隔开,故在后代码展示中则会忽略一部分,请自行补充
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="my_robot">
<static>true</static>
</model>
</sdf>
\qquad
② 创建长方体的底座
代码解释:
- box 标签,用于产生对应尺寸的长方体
- collision 标签,指定碰撞尺寸
- visual 标签,指定视觉尺寸,常见情况下与collision相同
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="my_robot">
<static>true</static>
<link name='chassis'>
<pose>0 0 .1 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.4 .2 .1</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.4 .2 .1</size>
</box>
</geometry>
</visual>
</link>
</model>
</sdf>
\qquad
③ 创建脚轮(万向轮)
注意:脚轮固定在底座上,故二者同属一个link
<collision name='caster_collision'>
<pose>-0.15 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
<slip1>1.0</slip1>
<slip2>1.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name='caster_visual'>
<pose>-0.15 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
</visual>
</link>
\qquad
④ 创建前轮和后轮
注意:前轮和后轮分别创建了新的link
<link name="left_wheel">
<pose>0.1 0.13 0.1 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_wheel">
<pose>0.1 -0.13 0.1 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
\qquad
⑤ 导入Gazebo可视化模型
- 使用INSERT导入对应文件夹的模型,可看到模型如下:
注意:修改SDF文件后,只需要删除原有模型重新插入就会更新模型
3. 创建关节连接部件(动态)
- 基本要求:将static设为false,为左右车轮添加铰链关节,关节绕Y轴旋转,将各车轮连接到底盘
<static>false</static>
<joint type="revolute" name="left_wheel_hinge">
<pose>0 0 -0.03 0 0 0</pose>
<child>left_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint type="revolute" name="right_wheel_hinge">
<pose>0 0 0.03 0 0 0</pose>
<child>right_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
4. Gazebo基本仿真
- 基本操作:启动Gazebo,插入最新模型,打开隐藏的右面板,选择好要控制的模型,如下图给Force下的各关节力进行修改,机器人就会发生移动:
二、创建Velodyne HDL-32 LiDAR传感器
- 内容简介:本节内容以一个两轮移动机器人为例,使用差动驱动机构运动,从无到有,使用SDF完成建模并在Gazebo中完成仿真,重点在于通过建模的细致流程掌握如何使用SDF文件和Gazebo软件完成机器人仿真。
1. 创建基本世界
gedit velodyne.world
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>
2. 创建传感器静态模型
<model name="velodyne_hdl-32">
<link name="base">
<pose>0 0 0.029335 0 0 0</pose>
<collision name="base_collision">
<geometry>
<cylinder>
<radius>.04267</radius>
<length>.05867</length>
</cylinder>
</geometry>
</collision>
<visual name="base_visual">
<geometry>
<cylinder>
<radius>.04267</radius>
<length>.05867</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="top">
<pose>0 0 0.095455 0 0 0</pose>
<collision name="top_collision">
<geometry>
<cylinder>
<radius>0.04267</radius>
<length>0.07357</length>
</cylinder>
</geometry>
</collision>
<visual name="top_visual">
<geometry>
<cylinder>
<radius>0.04267</radius>
<length>0.07357</length>
</cylinder>
</geometry>
</visual>
</link>
</model>
cd ~/
gazebo velodyne.world -u
- 查看碰撞属性:右键点击模型,view→Collisions
3. 添加模型惯性
- 3.1 查看当前惯性值:右键单击模型,选择View->Inertia
注意:紫色框对应关联的链接大小,此时模型没有惯性信息,故尺寸过大
- 3.2 添加惯性信息:质量设为1.3kg,添加对应质量和惯性矩阵
在< link name=“base” >块中添加以下内容:
<link name="base">
<pose>0 0 0.029335 0 0 0</pose>
<inertial>
<mass>1.2</mass>
<inertia>
<ixx>0.001087473</ixx>
<iyy>0.001087473</iyy>
<izz>0.001092437</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
在< link name=“top” >块中添加以下内容:
<link name="top">
<pose>0 0 0.095455 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000090623</ixx>
<iyy>0.000090623</iyy>
<izz>0.000091036</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
最终效果如下:
4. 添加关节
- 4.1 定义顶部围绕底部旋转关节,在< world >最后添加内容如下:
<joint type="revolute" name="joint">
<pose>0 0 -0.036785 0 0 0</pose>
<parent>base</parent>
<child>top</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-10000000000000000</lower>
<upper>10000000000000000</upper>
</limit>
</axis>
</joint>
1. 启动Gazebo,右键单击模型,选择View->Joints,View->Transparent
2. 打开右面板,选择Velodyne模型。使用Force选项卡向关节施加较小的,可看到关节旋转即可
5. 添加传感器
激光传感器,可以发出一个或多个光束,光束产生距离和强度数据,对应SDF文件中的< scan >和< range >,分别对应波束的布局、数量和限定束的性质,其中< scan >中包含< horizontal >和< vertical >两个块。< horizontal >组件定义在水平平面中发出的光线,该< vertical >组件定义在垂直平面中发出的光线,Velodyne传感器需要垂直射线,然后旋转。我们将其模拟为旋转的水平扇面。
<sensor type="ray" name="sensor">
<pose>0 0 -0.004645 1.5707 0 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<scan>
<horizontal>
<samples>32</samples>
<resolution>1</resolution>
<min_angle>-0.53529248</min_angle>
<max_angle>0.18622663</max_angle>
</horizontal>
</scan>
<range>
<min>0.05</min>
<max>70</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
在< sensor >的子标签< ray >中添加如下代码:
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.1</stddev>
</noise>
效果如下:
- 通过Ctrl+T打开topic visualization查看:
总结
- 内容分析:本篇博客主要介绍了在Gazebo中如何使用SDF进行手动的编程建模,通过编写SDF文件,实现对于机器人从无到有的一步步建造,体会SDF文件的语法使用,并在文章整体使用两个具体实例,分别是轮式小车和Velodyne HDL-32 LiDAR传感器模型进行了深入研究。
- 注意:本文参考了Gazebo官方网站以及古月居中的Gazebo有关教程,主要目的是方便自行查询知识,巩固学习经验,无任何商业用途。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)