第七章建模与仿真分为上、中、下三节。
第七章:机器人的建模与仿真(上)
一、URDF—机器人描述语言
URDF是机器人模型的描述格式,包括机器人刚体外观、物理属性、关节类型等。
1. URDF相关标签
在urdf中,描述机器人使用link,link之间使用joint连接
1.1 link
标签
刚体。
描述刚体外观和物理属性。
1.2joint
标签
关节。连接两个刚体。
描述机器人关节的运动学和动力学属性,包括关节运动的位置和速度限制
1.3 robot
标签
完整机器人的顶层标签。刚体与关节必须在该标签内。
1.4 gazebo
标签
描述机器人模型在gazebo仿真中的参数。包括材料属性、Gazebo插件等。
非必须标签。
2. 实例
手动写一遍urdf来实现一个机器人模型,对理解整个仿真机器人构造很有帮助
也会更加明白引入xacro的重要性.
<?xml version="1.0"?>
<robot name="my_robot1">
<material name="blue">
<!-- color range [0 - 1]-->
<color rgba="0 0 0.8 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2" />
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2" />
</geometry>
<!-- base your own frame -->
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25"/>
</joint>
<link name="right_base">
<visual>
<geometry>
<box size="0.4 0.1 0.1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="right_base_joint" type="fixed">
<parent link="right_leg"/>
<child link="right_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="right_front_wheel">
<visual>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
</link>
<joint name="right_front_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_front_wheel"/>
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
</joint>
<link name="right_back_wheel">
<visual>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
</link>
<joint name="right_back_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_back_wheel"/>
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2" />
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="0 0.22 0.25"/>
</joint>
<link name="left_base">
<visual>
<geometry>
<box size="0.4 0.1 0.1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="left_base_joint" type="fixed">
<parent link="left_leg"/>
<child link="left_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="left_front_wheel">
<visual>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
</link>
<joint name="left_front_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_front_wheel"/>
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
</joint>
<link name="left_back_wheel">
<visual>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
</link>
<joint name="left_back_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_back_wheel"/>
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
</joint>
<joint name="gripper_extension" type="fixed">
<parent link="base_link"/>
<child link="gripper_pole"/>
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
</joint>
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius="0.01"/>
</geometry>
<material name="white"/>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</visual>
</link>
<joint name="left_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<joint name="right_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="head_swivel" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size="0.08 0.08 0.08"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0.15 0 0.11"/>
</joint>
</robot>
launch 文件:
<launch>
<arg name="model" default="$(find learn_model)/urdf/my_robot.urdf"/>
<arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<!-- <param name="use_gui" value="$(arg gui)"/>-->
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
最终效果
参考教程
3. 相关tools/pack
3.1 验证工具
sudo apt install liburdfdom-tools
在终端输入如下命令:
check_urdf **.urdf
输出结果:
3.2 可视化工具
在终端输入如下命令:
urdf_to_graphiz **.urdf
在选定目录下生成节点连接的pdf文件:
没有问题!!
3.3 joint_state_publisher
节点发布每个joint的状态(除了fixed类型),还可以通过UI对joint进行控制
3.4 robot_state_publisher
将机器人各个link,joint之间的关系,通过TF的形式整理成三维姿态发布
会订阅joint_state_publisher
节点发布的joint_states
话题。
订阅参数robot_description
,所以需要roslaunch的时候提供对应参数。
如果参数有变的话,可以使用<remap>
标签重映射,如下例:
<launch>
<param name="my_robot_description" textfile="$(find mrobot_description)/urdf/mrobot_chassis.urdf" />
<!-- 运行robot_state_publisher节点,发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<remap from="robot_description" to="my_robot_discription"/>
</node>>
</launch>
二、让机器人动起来
机器人动起来最关键的关节joint标签。
joint标签最后需要指定type, 所有的type有如下几种:
- continuous: 旋转关节
- revolute: 旋转关节,角度有限制
- prismatic: 滑动关节,有位置极限
- fixed: 固定关节.
- floating: 浮动关节
- planar: 平面关节
之前我们的机器人模型,都是用的fixed固定关节.
本小节,笔者会探索另外几种重要的关节类型.
the head 的 continuous关节
机器人的头: 属于continuous关节
<joint name="head_swivel" type="continuous">
<parent link="base_link"/>
<child link="head"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.3"/>
</joint>
按照上面描述,这颗头可以沿着z轴360度旋转. 我们来看看效果:
gripper的revolute关节
前面的抓手, 属于rebolute关节,他们也可以沿着某个轴转动,但是有角度限制
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
limit
标签就是用来限制角度的.
看看效果:
gripper ARM 的 prismatic标签
抓手的手臂属于滑动标签,而且限制滑动范围.
看看urdf怎么写:
<joint name="gripper_extension" type="prismatic">
<parent link="base_link"/>
<child link="gripper_pole"/>
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
</joint>
看看效果:
最后的两个标签解释:
沿着对prismatic的理解,我们将移动范围扩大到二维和三维空间之后,就是另外的两种关节移动方法.
floating: 浮动关节是在三维空间任意移动
planar: 平面关节是在平面空间任意移动
最后
思考下为什么移动滑窗能控制相关关节的移动?
回答:
首先GUI解析URDF找到所有非固定关节以及他们的活动范围.
然后GUI使用滑窗发布message:sensor_msgs/JointState
robot_state_publisher
订阅该message,计算各个部分的TF
最终的tf_tree以及刚体形状被rviz可视化出来
三、添加物理与碰撞属性
1 碰撞属性
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
</link>
collision
标签直接放在link下,与visual
标签同等级。collision
标签可以设置更大的外围范围,比如保护头的时候,可以比原本的geometry大一圈;同样复杂的meshes可以用简单的geometry包裹,这样有利于快速的碰撞检测。
2 物理属性
1. 惯性:
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
</inertial>
</link>
inertial
元素,同样与visual
同等级mass
单位为kginertia
元素中的6个数字分别代表如下矩阵:
具体的物理含义是惯性张量。对角元素是分别对x,y,z三轴的转动惯量,非对角元素称为惯量积。
矩阵用于物体对其旋转运动的惯性大小的度量。
具体解释参考维基百科
其他标签:
对于link而言,在碰撞collision
tag下还有其他属性可以设置:
- mu: 摩擦系数
- kp:刚体系数
- kd:阻尼系数
对于joint而言,有如下物理属性:
- friction: 摩擦
- damping:阻尼
四、xacro语言简化URDF模型
在我们生成机器人URDF过程中,一定会对很多对称的重复的定义感到繁琐。针对URDF中冗长重复的定义,xacro语言作为一种macro语言可以实现对urdf的简化。
1. constants常量定义
很多尺寸坐标等常量的使用,使得原本的URDF模型可读性极差,并且难以维护。
xacro提供一种常量属性的定义方式:
<xacro:property name="width" value="0.2" />
我们先用常量来给前文中的urdf瘦身。查看如下原来的base_link:
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
</link>
使用常量的base_link:
<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${width}" length="${bodylen}"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="${width}" length="${bodylen}"/>
</geometry>
</collision>
</link>
两行属性的定义可以出现在xacro文件中的任何位置。
2. 数学公式
<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />
在${}
符号中,除了调用常量,还可以使用一些常用的数学运算:+ - * / () sin cos
等
3. Macros宏定义
像写函数一样,宏定义声明基于输入的参数,可以重复使用代码模块。
比如一辆车有四个轮子,我们写一个轮子的定义够了。其他只是参数的不同。
我们看一下,前文中机器人的两条腿,来实现代码复用。
<xacro:macro name="leg" params="prefix reflect">
<link name="${prefix}_leg">
<visual>
<geometry>
<box size="${leglen} 0.1 0.2"/>
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="${leglen} 0.1 0.2"/>
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
</collision>
<xacro:default_inertial mass="10"/>
</link>
<joint name="base_to_${prefix}_leg" type="fixed">
<parent link="base_link"/>
<child link="${prefix}_leg"/>
<origin xyz="0 ${reflect*(width+.02)} 0.25" />
</joint>
<!-- A bunch of stuff cut -->
</xacro:macro>
<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />
trick
- 使用名称前缀命名两个相似物体
- 使用math计算关节朝向
- 使用镜像参数,
1
和-1
实现两条腿对称
其他有用的特性
- 如果传入的参数为属性块,需要在参数栏对应参数名字前添加
*
号 - 在对应的宏定义块中,插入属性块部分需要使用
insert_block
命令
举个例子:
<xacro:macro name="blue_shape" params="name *shape">
<link name="${name}">
<visual>
<geometry>
<xacro:insert_block name="shape" />
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<xacro:insert_block name="shape" />
</geometry>
</collision>
</link>
</xacro:macro>
在定义blue_shape的时候,由于传入的参数为属性块,需要在参数栏对应参数名字shape
前添加*
号
同样在插入属性块部分需要使用insert_block
命令:
<xacro:insert_block name="shape" />
在使用blue_shape宏定义的时候,需要传入两种参数,一个是name
,一个是从属性块*shape
。 如下所示:
<xacro:blue_shape name="base_link">
<cylinder radius=".42" length=".01" />
</xacro:blue_shape>
4. 使用xacro语言
使用命令行可以直接将xacro文件解析为urdf文件
xacro model.xacro > model.urdf
在launch文件中,我们同样可以使用对应命令生成urdf文件
<param name="robot_description"
command="xacro --inorder '$(find pr2_description)/robots/pr2.urdf.xacro'" />