“热血的征程使我们陶醉!”
- 延用工作空间test2_ws;
- 功能包demo3_urdf存放代码;
- 介绍link的代码放在demo4_urdf/urdf/urdf/urdf_rviz4_robot.urdf中,
相关代码放在demo3_urdf/launch/urdf_rviz4_robot.launch;
作业内容
创建一个四轮车,,底盘为圆柱状,半径 10cm,高 8cm;
四轮由两个驱动轮和两个万向支撑轮组成,
两个驱动轮半径为 3.25cm,轮胎宽度1.5cm;
两个万向轮为球状,半径 0.75cm,底盘离地间距为 1.5cm(与万向轮直径一致)
思路
创建机器人模型可以分步骤实现:
1.新建 urdf 文件,并与 launch 文件集成
2.搭建底盘
3.底盘上添加两个驱动轮
4.在底盘上添加两个万向轮
源码
urdf文件
<robot name="mycar">
<!-- base_footprint -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<!--
参数
形状:圆柱
半径:10 cm
高度:8 cm
离地:1.5 cm
-->
<!-- base_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>
<!-- 添加两个驱动轮 -->
<!--
驱动轮是侧翻的圆柱
参数
半径: 3.25 cm
宽度: 1.5 cm
颜色: 黑色
关节设置:
x = 0
y = 底盘的半径 + 轮胎宽度 / 2
z = 离地间距 + 底盘长度 / 2 - 轮胎半径 = 1.5 + 4 - 3.25 = 2.25(cm)
axis = 0 1 0
-->
<!-- two drive whell -->
<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>
<!-- 添加万向轮(支撑轮) -->
<!--
参数
形状: 球体
半径: 0.75 cm
颜色: 黑色
关节设置:
x = 自定义(底盘半径 - 万向轮半径) = 0.1 - 0.0075 = 0.0925(cm)
y = 0
z = 底盘长度 / 2 + 离地间距 / 2 = 0.08 / 2 + 0.015 / 2 = 0.0475
axis= 1 1 1
-->
<!-- direction(hold) circle -->
<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 demo3_urdf_rviz)/urdf/urdf/urdf_rviz4_robot.urdf" />
<!-- 启动 rivz -->
<node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find demo3_urdf_rviz)/config/rviz/urdf_rviz3_joint.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>
检查工具
check_urdf
在urdf文件目录下,执行check_urdf 文件名.urdf
可以检查
如果无误,输出successfully 。
生成结构图urdf_to_graphiz
在urdf文件目录下,执行urdf_to_graphiz urdf_rviz4_robot.urdf
可以在该目录下生成pdf文件,打开如图:
最后
祝大家元旦快乐!