最近我在古月学院购买了在Gazebo中实现阿克曼转向车的仿真课程,课程资料的阿克曼小车的XACRO文件在Gazebo里加载的时候报了三个错误,在这之前,我没接触过URDF或XACRO文件,本篇文章就以这个阿克曼小车的XACRO文件为例,对URDF或XACRO文件的格式和内容进行解读,期望经过解读和学习,找到错误原因及所在,并解决这三个错误
先来放一下上面右图所示的模型的完整的XACRO文件:
<robot name="racecar" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find racecar_description)/urdf/sensors/lidar.xacro" />
<xacro:include filename="$(find racecar_description)/urdf/sensors/imu.xacro" />
<xacro:include filename="$(find racecar_description)/urdf/sensors/camera.xacro" />
<xacro:property name="degrees_45" value="0.785398163"/>
<xacro:property name="degrees_90" value="1.57079633"/>
<xacro:property name="chassis_length" value="0.258"/>
<xacro:property name="chassis_width" value="0.168"/>
<xacro:property name="chassis_height" value="0.01"/>
<xacro:property name="chassis_mass" value="2.788"/>
<xacro:property name="battery_x_offset" value="0.055"/>
<xacro:property name="battery_dist" value="0.065"/>
<xacro:property name="battery_comp_depth" value="0.02"/>
<xacro:property name="battery_comp_angle" value="0.34906585"/>
<xacro:property name="battery_length" value="0.16"/>
<xacro:property name="battery_width" value="0.047"/>
<xacro:property name="battery_height" value="0.024"/>
<xacro:property name="battery_mass" value="0.5025"/>
<xacro:property name="hub_dia" value="0.09652"/>
<xacro:property name="tire_dia" value="0.14605"/>
<xacro:property name="tire_width" value="0.0889"/>
<xacro:property name="hex_hub_depth" value="0.01445"/>
<xacro:property name="wheel_mass" value="0.29"/>
<xacro:property name="wheelbase" value="0.335"/>
<xacro:property name="hex_hub_dist" value="0.365"/>
<xacro:property name="axle_length" value="0.03"/>
<xacro:property name="wheel_travel" value="0.084"/>
<xacro:property name="shock_z_offset" value="0.0655"/>
<xacro:property name="shock_eff_limit" value="12.5106"/>
<xacro:property name="shock_vel_limit" value="1000"/>
<xacro:property name="axle_eff_limit" value="5.12766"/>
<xacro:property name="axle_vel_limit" value="244.8696"/>
<xacro:property name="servo_stall_torque" value="0.5649"/>
<xacro:property name="servo_no_load_speed" value="4.553"/>
<xacro:property name="layer_mass" value="0.0" />
<xacro:property name="layer_x" value="0.40"/>
<xacro:property name="layer_y" value="0.18"/>
<xacro:property name="layer_z" value="0.01"/>
<xacro:property name="layer_height" value="0.08"/>
<material name="battery_mat">
<color rgba="0 0 1 1"/>
material>
<material name="chassis_mat">
<color rgba="0.5 0.5 0.5 1"/>
material>
<material name="tire_mat">
<color rgba="0 0 0 1"/>
material>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
material>
<xacro:macro name="null_inertial">
<inertial>
<mass value="0.001"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
inertial>
xacro:macro>
<xacro:macro name="solid_cuboid_inertial"
params="width depth height mass *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}"/>
<inertia ixx="${mass * (depth * depth + height * height) / 12}"
ixy="0" ixz="0"
iyy="${mass * (width * width + height * height) / 12}"
iyz="0"
izz="${mass * (width * width + depth * depth) / 12}"/>
inertial>
xacro:macro>
<xacro:macro name="thick_walled_tube_inertial"
params="inner_rad outer_rad height mass">
<inertial>
<mass value="${mass}"/>
<inertia ixx="${(1 / 12) * mass * (3 * (inner_rad * inner_rad +
outer_rad * outer_rad) + height * height)}"
ixy="0" ixz="0"
iyy="${(1 / 12) * mass * (3 * (inner_rad * inner_rad +
outer_rad * outer_rad) + height * height)}"
iyz="0"
izz="${mass * (inner_rad * inner_rad +
outer_rad * outer_rad) / 2}"/>
inertial>
xacro:macro>
<xacro:macro name="battery" params="prefix reflect">
<joint name="chassis_to_${prefix}_battery" type="fixed">
<parent link="chassis"/>
<child link="${prefix}_battery_offset"/>
<origin xyz="${battery_x_offset - battery_length / 2}
${reflect * battery_dist / 2}
0"
rpy="${reflect * battery_comp_angle} 0 0"/>
joint>
<link name="${prefix}_battery_offset">
<xacro:null_inertial/>
link>
<joint name="offset_to_${prefix}_battery" type="fixed">
<parent link="${prefix}_battery_offset"/>
<child link="${prefix}_battery"/>
<origin xyz="0
${reflect * battery_width / 2}
${(battery_height / 2) - battery_comp_depth}"/>
joint>
<link name="${prefix}_battery">
<visual>
<geometry>
<box size="${battery_length} ${battery_width} ${battery_height}"/>
geometry>
<material name="battery_mat"/>
visual>
<collision>
<geometry>
<box size="${battery_length} ${battery_width} ${battery_height}"/>
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${battery_length}" depth="${battery_width}"
height="${battery_height}" mass="${battery_mass}">
<origin/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="${prefix}_battery">
<material>Gazebo/Bluematerial>
gazebo>
xacro:macro>
<xacro:macro name="shock"
params="lr_prefix fr_prefix lr_reflect fr_reflect child">
<joint name="${lr_prefix}_${fr_prefix}_shock" type="prismatic">
<parent link="chassis"/>
<child link="${child}"/>
<origin xyz="${fr_reflect * wheelbase / 2}
${lr_reflect * ((hex_hub_dist / 2) - axle_length)}
${(wheel_travel / 2) - shock_z_offset}"/>
<axis xyz="0 0 -1"/>
<limit lower="${-wheel_travel / 2}" upper="${wheel_travel / 2}"
effort="${shock_eff_limit}" velocity="${shock_vel_limit}"/>
joint>
<transmission name="${lr_prefix}_${fr_prefix}_shock_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_${fr_prefix}_shock">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_${fr_prefix}_shock_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
xacro:macro>
<xacro:macro name="wheel" params="lr_prefix fr_prefix lr_reflect">
<link name="${lr_prefix}_${fr_prefix}_axle_carrier">
<xacro:null_inertial/>
link>
<joint name="${lr_prefix}_${fr_prefix}_axle" type="continuous">
<parent link="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<child link="${lr_prefix}_${fr_prefix}_wheel"/>
<origin rpy="${degrees_90} 0 0"/>
<axis xyz="0 0 -1"/>
<limit effort="${axle_eff_limit}" velocity="${axle_vel_limit}"/>
joint>
<transmission name="${lr_prefix}_${fr_prefix}_axle_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_${fr_prefix}_axle">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_${fr_prefix}_axle_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<link name="${lr_prefix}_${fr_prefix}_wheel">
<visual>
<origin xyz="0
${lr_reflect * (axle_length - (tire_width /
2 - hex_hub_depth))}
0"/>
<geometry>
<cylinder radius="${tire_dia / 2}" length="${tire_width}"/>
geometry>
<material name="tire_mat"/>
visual>
<collision>
<origin xyz="0
${lr_reflect * (axle_length - (tire_width /
2 - hex_hub_depth))}
0"/>
<geometry>
<cylinder radius="${tire_dia / 2}" length="${tire_width}"/>
geometry>
collision>
<xacro:thick_walled_tube_inertial
inner_rad="${hub_dia / 2}" outer_rad="${tire_dia / 2}"
height="${tire_width}" mass="${wheel_mass}"/>
link>
<gazebo reference="${lr_prefix}_${fr_prefix}_wheel">
<material>Gazebo/Blackmaterial>
gazebo>
xacro:macro>
<xacro:macro name="front_wheel"
params="lr_prefix fr_prefix lr_reflect fr_reflect">
<xacro:shock lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}" fr_reflect="${fr_reflect}"
child="${lr_prefix}_steering_link"/>
<link name="${lr_prefix}_steering_link">
<xacro:null_inertial/>
link>
<joint name="${lr_prefix}_steering_joint" type="revolute">
<parent link="${lr_prefix}_steering_link"/>
<child link="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<axis xyz="0 0 1"/>
<limit lower="${-degrees_45}" upper="${degrees_45}"
effort="${servo_stall_torque}" velocity="${servo_no_load_speed}"/>
joint>
<transmission name="${lr_prefix}_steering_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_steering_joint">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_steering_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<xacro:wheel lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}"/>
xacro:macro>
<xacro:macro name="rear_wheel"
params="lr_prefix fr_prefix lr_reflect fr_reflect">
<xacro:shock lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}" fr_reflect="${fr_reflect}"
child="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<xacro:wheel lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}"/>
xacro:macro>
<xacro:macro name="layer" params="layer parent m x y z *joint_pose">
<joint name="${layer}_joint" type="fixed">
<xacro:insert_block name="joint_pose"/>
<parent link="${parent}"/>
<child link="${layer}_link" />
joint>
<link name="${layer}_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${x} ${y} ${z}" />
geometry>
<material name="yellow" />
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${x} ${y} ${z}" />
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${x}" depth="${y}"
height="${z}" mass="${m}">
<origin xyz="0 0 ${-z / 2}"/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="${layer}_link">
<material>Gazebo/Orangematerial>
gazebo>
xacro:macro>
<link name="base_footprint"/>
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.01"/>
geometry>
<material name="chassis_mat"/>
visual>
link>
<gazebo reference="base_link">
<material>Gazebo/Greymaterial>
gazebo>
<joint name="base_footprint_to_base_link" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 ${(wheel_travel/2)+shock_z_offset}" />
joint>
<link name="chassis">
<visual>
<origin xyz="0 0 ${-chassis_height / 2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
geometry>
<material name="chassis_mat"/>
visual>
<collision>
<origin xyz="0 0 ${-chassis_height / 2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${chassis_length}" depth="${chassis_width}"
height="${chassis_height}" mass="${chassis_mass}">
<origin xyz="0 0 ${-chassis_height / 2}"/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="chassis">
<material>Gazebo/Greymaterial>
gazebo>
<joint name="base_link_to_chassis" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
joint>
<xacro:battery prefix="left" reflect="1"/>
<xacro:battery prefix="right" reflect="-1"/>
<xacro:front_wheel lr_prefix="left" fr_prefix="front"
lr_reflect="1" fr_reflect="1"/>
<xacro:front_wheel lr_prefix="right" fr_prefix="front"
lr_reflect="-1" fr_reflect="1"/>
<xacro:rear_wheel lr_prefix="left" fr_prefix="rear"
lr_reflect="1" fr_reflect="-1"/>
<xacro:rear_wheel lr_prefix="right" fr_prefix="rear"
lr_reflect="-1" fr_reflect="-1"/>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<legacyModeNS>truelegacyModeNS>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
<robotNamespace>/racecarrobotNamespace>
plugin>
gazebo>
<layer layer="platform" parent="chassis" m="${layer_mass}" x="${layer_x}" y="${layer_y}" z="${layer_z}">
<origin xyz="0 0 ${layer_height}" rpy="0 0 0" />
layer>
<layer layer="lidar_platform" parent="platform_link" m="${layer_mass}" x="0.12" y="0.10" z="0.01">
<origin xyz="0.08 0 ${layer_height}" rpy="0 0 0" />
layer>
<joint name="lidar_joint" type="fixed">
<origin xyz="0 0 0.03" rpy="0 0 0" />
<parent link="lidar_platform_link"/>
<child link="laser_link"/>
joint>
<lidar prefix="laser"/>
<joint name="imu_joint" type="fixed">
<origin xyz="0.16 0 0.015" rpy="0 0 0" />
<parent link="platform_link"/>
<child link="imu_link"/>
joint>
<imu prefix="imu"/>
<joint name="camera_joint" type="fixed">
<origin xyz="0.16 0 0.05" rpy="0 0 0" />
<parent link="platform_link"/>
<child link="camera_link"/>
joint>
<camera prefix="camera"/>
robot>
接下来我们按顺序对该XACRO文件进行解读
第一部分:模型介绍及许可证说明
该部分内容会对该模型进行简单的介绍和说明,比如本文中的例子,介绍了这个文件定义了一个Traxxas® E-Maxx® #3905 RC(无线电控制)的模型小车。它的长度的单位是米,角度的单位是弧度,质量的单位是以公斤为单位。所有这些数值都是近似值。为了与Gazebo一起工作,每个链接必须有一个惯性元件,即使是惯性元素,即使该链接仅用于连接两个关节点。为了在Gazebo中可见,一个链接必须有一个碰撞元素。此外,该链接必须有一个Gazebo材料等信息.后面就是一些许可证说明了。
第二部分: < robot > 标签
<robot name="racecar" xmlns:xacro="http://www.ros.org/wiki/xacro">
...
robot>
< robot > 标签是完整机器人模型的最顶层标签 < link >和 < joint > 标签都必须包含在 < robot > 标签内,一个完整的机器人模型,由一系列< link >和< joint >组成
在本例中的robot标签中定义了该模型的名字racecar,后面的xmlns:xacro="http://www .ros.org/wiki/xacro"是命名空间声明。
xacro 提供了可编程接口,类似于计算机语言,包括变量声明调用、函数声明与调用等语法实现。在使用 xacro 生成 urdf 时,根标签robot中必须包含命名空间声明:xmlns:xac ro=“http://wiki.ros.org/xacro”
第三部分: 子程序调用
<xacro:include filename="$(find racecar_description)/urdf/sensors/lidar.xacro" />
<xacro:include filename="$(find racecar_description)/urdf/sensors/imu.xacro" />
<xacro:include filename="$(find racecar_description)/urdf/sensors/camera.xacro" />
在XACRO文件中可以通过
在本例中调用了激光雷达、惯性IMU、相机这三个子程序
第四部分:属性/属性块定义
<xacro:property name="degrees_45" value="0.785398163"/>
<xacro:property name="degrees_90" value="1.57079633"/>
<xacro:property name="chassis_length" value="0.258"/>
<xacro:property name="chassis_width" value="0.168"/>
<xacro:property name="chassis_height" value="0.01"/>
<xacro:property name="chassis_mass" value="2.788"/>
<xacro:property name="battery_x_offset" value="0.055"/>
<xacro:property name="battery_dist" value="0.065"/>
<xacro:property name="battery_comp_depth" value="0.02"/>
<xacro:property name="battery_comp_angle" value="0.34906585"/>
<xacro:property name="battery_length" value="0.16"/>
<xacro:property name="battery_width" value="0.047"/>
<xacro:property name="battery_height" value="0.024"/>
<xacro:property name="battery_mass" value="0.5025"/>
<xacro:property name="hub_dia" value="0.09652"/>
<xacro:property name="tire_dia" value="0.14605"/>
<xacro:property name="tire_width" value="0.0889"/>
<xacro:property name="hex_hub_depth" value="0.01445"/>
<xacro:property name="wheel_mass" value="0.29"/>
<xacro:property name="wheelbase" value="0.335"/>
<xacro:property name="hex_hub_dist" value="0.365"/>
<xacro:property name="axle_length" value="0.03"/>
<xacro:property name="wheel_travel" value="0.084"/>
<xacro:property name="shock_z_offset" value="0.0655"/>
<xacro:property name="shock_eff_limit" value="12.5106"/>
<xacro:property name="shock_vel_limit" value="1000"/>
<xacro:property name="axle_eff_limit" value="5.12766"/>
<xacro:property name="axle_vel_limit" value="244.8696"/>
<xacro:property name="servo_stall_torque" value="0.5649"/>
<xacro:property name="servo_no_load_speed" value="4.553"/>
<xacro:property name="layer_mass" value="0.0" />
<xacro:property name="layer_x" value="0.40"/>
<xacro:property name="layer_y" value="0.18"/>
<xacro:property name="layer_z" value="0.01"/>
<xacro:property name="layer_height" value="0.08"/>
< xacro:property name=" **** " value= " **** " />语句相当于C语言中的参数定义,name后的值相当于参数名,value后的值相当于参数值。
属性是可以插入到XML文档中的任何位置的值。 属性块是XML的名称片段,可以插入允许XML的任何位置。 两者都使用属性标签来定义值,可以用insert_block将属性块插入到合适的位置
第五部分:颜色设置
<material name="battery_mat">
<color rgba="0 0 1 1"/>
material>
<material name="chassis_mat">
<color rgba="0.5 0.5 0.5 1"/>
material>
<material name="tire_mat">
<color rgba="0 0 0 1"/>
material>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
material>
< material name="…" > 语句定义一个颜色变量名…,可为下面的结构设置颜色,< color rgba=“0 0 1 1”/ > 对上面那条语句设定的颜色变量,进行颜色设置,后面四个参数依次为R 、G、 B颜色设置及透明度设置
第六部分:宏定义
< !-- Macro for inertia matrix -->
…
格式用于进行宏定义,相当于C语言里面的函数,其中macro中的name相当于规定函数名,param相当于形参名。
接下来我们依次对本例中定义的众多宏进行解读,由于大家可能对函数这个词更加熟悉,下面对宏的介绍,用函数这个词来代替,来帮助大家更容易理解
1、null_inertial
<xacro:macro name="null_inertial">
<inertial>
<mass value="0.001"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
inertial>
xacro:macro>
这个“函数”,进行了一些惯量的设置,标签< inertia l> 用来对惯量进行设置,其中< mass value=“0.05”/ > 是对质量进行了设置,单位是Kg ,< inertia ixx=“0.001” ixy=“0” ixz=“0” iyy=“0.001” iyz=“0” izz=“0.001”/ > 设置了转动惯量矩阵,需要刚体动力学基础
这样后续再需要进行如上的惯量设置的时候,就不用再写这么多了,只需要通过语句< xacro:null_inertial/ > 来调用这个“函数”就可以了
2、solid_cuboid_inertial
<xacro:macro name="solid_cuboid_inertial"
params="width depth height mass *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}"/>
<inertia ixx="${mass * (depth * depth + height * height) / 12}"
ixy="0" ixz="0"
iyy="${mass * (width * width + height * height) / 12}"
iyz="0"
izz="${mass * (width * width + depth * depth) / 12}"/>
inertial>
xacro:macro>
这个“函数”,是带有“形参”的,其中最后一个是以属性块的形式通过insert_block插入,调用这带有“形参”的“函数”就不能像上面那样只调用“函数”名就行了,还需要进行参数传递,如:
height="$ {z}" mass="$ {m}">
< origin xyz=“0 0 ${-z / 2}”/>
< /xacro:solid_cuboid_inertial>
是对这个函数的调用,在函数内部使用形参或者调用时使用的时候用 ${形参名} 的形式,支持四则运算。
调用自身文件内macro的方式:< macro名 macro.param=**** />
调用外部文件的macro的方式:< xacro:include filename= " $(find 包名)/urdf/xacro名.xacro" / > < macro名 macro.param=****/>
这个函数对实体长方体的惯性进行了设置。宽度沿x轴测量,深度沿着Y轴测量,而高度则沿着Z轴测量。
3、thick_walled_tube_inertial
<xacro:macro name="thick_walled_tube_inertial"
params="inner_rad outer_rad height mass">
<inertial>
<mass value="${mass}"/>
<inertia ixx="${(1 / 12) * mass * (3 * (inner_rad * inner_rad +
outer_rad * outer_rad) + height * height)}"
ixy="0" ixz="0"
iyy="${(1 / 12) * mass * (3 * (inner_rad * inner_rad +
outer_rad * outer_rad) + height * height)}"
iyz="0"
izz="${mass * (inner_rad * inner_rad +
outer_rad * outer_rad) / 2}"/>
inertial>
xacro:macro>
这个“函数”,是对两端开口的厚壁圆柱管的惯性进行设定。其中高度是沿着Z轴测量,它是管子的轴线。 inner_rad和 outer_rad分别是管子的内半径和外半径。
4、battery
<xacro:macro name="battery" params="prefix reflect">
<joint name="chassis_to_${prefix}_battery" type="fixed">
<parent link="chassis"/>
<child link="${prefix}_battery_offset"/>
<origin xyz="${battery_x_offset - battery_length / 2}
${reflect * battery_dist / 2}
0"
rpy="${reflect * battery_comp_angle} 0 0"/>
joint>
<link name="${prefix}_battery_offset">
<xacro:null_inertial/>
link>
<joint name="offset_to_${prefix}_battery" type="fixed">
<parent link="${prefix}_battery_offset"/>
<child link="${prefix}_battery"/>
<origin xyz="0
${reflect * battery_width / 2}
${(battery_height / 2) - battery_comp_depth}"/>
joint>
<link name="${prefix}_battery">
<visual>
<geometry>
<box size="${battery_length} ${battery_width} ${battery_height}"/>
geometry>
<material name="battery_mat"/>
visual>
<collision>
<geometry>
<box size="${battery_length} ${battery_width} ${battery_height}"/>
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${battery_length}" depth="${battery_width}"
height="${battery_height}" mass="${battery_mass}">
<origin/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="${prefix}_battery">
<material>Gazebo/Bluematerial>
gazebo>
xacro:macro>
这个“函数”,中用到了标签 < link > 和 标签 < joint >,接下来先来介绍一下这两个重要的标签:
下图很形象的将URDF要定义的主要内容给展现了出来. 一般,机器人都是由link和joint进行描述. 都会呈现为树状(想象数据结构里面的树),jiont用于连接两个link
为了能够确定位置,我们可以使用origin子标签进行定义link所应该在的位置. 但是有一点应该注意到, link和link之间是使用joint进行连接, 那么link的位置, 就由连接他的joint确定. 所以, 该子标签是定义在joint内,在使用joint时,还要指定父节点和子节点 ,用标签< parent > 和 < child > 进行设定,配置joint的时候,还要通过type进行连接方式的设定,比如固定关节fixed,转动关节revolute,旋转关节continuous,自由度浮动关节floating,或平面运动关节planar等
而且link不是个质点,而是个刚体所以光有描述点的x,y,z是不够用的, 还需要使用rpy. rpy角是描述船舶在海中航行时姿态的一种方法. 将船的行驶方向取为z轴, 绕z轴旋转称为滚动(Roll), 绕y轴旋转称为俯仰(Pitch), 绕x轴旋转称为偏转(Yaw). 这种描述方式大量运用于各个领域.(z是指机器人的上方,y是指机器人的左侧,x是指右侧,rpy中第一个参数是绕x轴转的角度,第二个为y,第三个是z,注意这里的角度是弧度值)
除了设定每个link的相对位置, 还要设定每个link的形状 比如说box表示长方体,cylinder是圆柱,sphere球
除此之外我们还可以对jiont的旋转或者移动的轴进行设定,比如, 我们限定joint只能沿着y轴旋转, 则需要添加, 类似的, 可以指定其他关节的转动轴, 例如< axis xyz="-0.5 0.6 1"/>.
我们还可以设定范围,比如, 我们要限定joint的移动范围, 可需要添加,如, 对上下限以及速度, 力矩等进行设定
< visual > 标签包围起来的部分,为在rviz中的可见部分
< geometry >标签包围起来的部分,为这个结构的形状。比如cylinder、box、sphere等形状
前面也提到了,如果想在Gazebo或者其他仿真软件中进行机器人仿真,就需要添加物理属性和碰撞属性,标签< collision > 用于设定“碰撞”,体积定义在仿真时,对应结构的物理体积,一般使它和geometry中的参数设置一致。其他情况:需要更快计算碰撞检测时可将复杂结构的碰撞体积定义为比较简单的几何形状,或者为了限制行为接近敏感设备时将碰撞体积设置较大来增加安全区域
有了以上的基础,我们来看一下这个“函数”的作用,该函数有两个形参prefix和reflect,这两个传入的形参影响了,调用该函数生成的link和joint的名字,该函数内部依次创建了,连接chassis_to_$ { prefix } _ battery,他的父节点是chassis,子节点是 $ { prefix } _ battery_offset,接着便创建了名为 $ { prefix } _ battery_offset的节点,在设置该节点的惯性时调用了,前面介绍过的函数null_inertial,接下来创建了名为offset_to_ $ { prefix}_battery的连接,该连接的父节点是 $ { prefix } _ battery _ offset,子节点是$ { prefix } _ battery, 接下来就创建了这个子节点$ { prefix } _ battery,并对该节点的可见部分,几何形状,碰撞等属性进行了设定,在设定惯性的时候,调用了前面介绍过的函数 solid_cuboid_inertial,最后为颜色加入gazebo标签。
5、shock
<xacro:macro name="shock"
params="lr_prefix fr_prefix lr_reflect fr_reflect child">
<joint name="${lr_prefix}_${fr_prefix}_shock" type="prismatic">
<parent link="chassis"/>
<child link="${child}"/>
<origin xyz="${fr_reflect * wheelbase / 2}
${lr_reflect * ((hex_hub_dist / 2) - axle_length)}
${(wheel_travel / 2) - shock_z_offset}"/>
<axis xyz="0 0 -1"/>
<limit lower="${-wheel_travel / 2}" upper="${wheel_travel / 2}"
effort="${shock_eff_limit}" velocity="${shock_vel_limit}"/>
joint>
<transmission name="${lr_prefix}_${fr_prefix}_shock_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_${fr_prefix}_shock">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_${fr_prefix}_shock_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
xacro:macro>
要想看懂这个宏的作用,还需要补充了解以下内容:
传动元素是URDF机器人描述模型的一个扩展,用于描述执行器和关节之间的关系。这允许人们对齿轮比和平行连接等概念进行建模。传动装置对努力/流量变量进行转换,使其乘积–功率–保持不变。多个执行器可以通过复杂的传动装置与多个关节相连。机器人每个需要运动的关节都需要配置相应的Transmission,下面是一个传动元件的例子。
< transmission name="$ ,{ lr_prefix} _ $ { fr _ prefix } _ shock _ trans" > 通过name对传动元件的名字进行了设定; < type >transmission _ interface / Simple Transmission < / type >,通过type对其传输类型进行了设定(只能指定一种), < joint name =" $ {lr _ pre fix}_ $ { fr_prefix }_shock"> < hardwareInterface > hardware _ interface /EffortJointInterface < /hardwareInterface > < /join >,通过joint对该传动元件的所连的关节进行设定(可以一个或多个)。
通过jiont的子元素hardwareInterface指定一个支持的联合空间硬件接口,当此传送在Gazebo中加载时,此标签的值应该是EffortJointInterface,当此传送在RobotHW中加载时,标签值为hardware_inter face / EffortJointInterface,此处我们用于在Gazebo中加载,但是例子中设定的却是hardware_inter face / EffortJointInterface先存个疑吧。
接下来的 < actuator>标签用于设定传动装置所连接的执行器(一个或多个)。该执行器由其名称属性和以下子元素指定,子标签< mechanicalReduction > (可选)用于设定接头/执行器传动处的机械减速。对于所有的传动装置来说,可能不需要这个标签。子标签< hardwareInterface > (可选) ,用于指定一个支持的联合空间硬件接口。 请注意,这里的< hardwareInterface >标签只应在Indigo之前的ROS版本中指定。指定这个标签的正确位置是在< joint >标签中。本文的例子中按理说不应该在此位置存在该标签,但是文中的注释说这个硬件接口元素的存在是为了兼容与ROS Hydro兼容,这个地方也存个疑吧。
我们来看一下这个“函数”的作用,先创建了一个名为$ {lr_prefix}_ $ {fr_prefix} _ shock的连接,同样该链接的名字由传入的参数决定,该连接的父节点同样是chassis,子节点的名字由传入的参数决定 ${child},接下来对该关节的位姿、转轴、限制等进行了设置,接下来设置了一个传动元件 $ { lr_prefix}_${fr_prefix}_shock_trans,它连接在之前创建的 $ {lr_prefix} _ $ {fr_prefix} _ shock上,并连接了一个执行器 $ {lr_prefix } _ $ {fr_prefix} _ shock _ act
6、wheel
<xacro:macro name="wheel" params="lr_prefix fr_prefix lr_reflect">
<link name="${lr_prefix}_${fr_prefix}_axle_carrier">
<xacro:null_inertial/>
link>
<joint name="${lr_prefix}_${fr_prefix}_axle" type="continuous">
<parent link="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<child link="${lr_prefix}_${fr_prefix}_wheel"/>
<origin rpy="${degrees_90} 0 0"/>
<axis xyz="0 0 -1"/>
<limit effort="${axle_eff_limit}" velocity="${axle_vel_limit}"/>
joint>
<transmission name="${lr_prefix}_${fr_prefix}_axle_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_${fr_prefix}_axle">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_${fr_prefix}_axle_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<link name="${lr_prefix}_${fr_prefix}_wheel">
<visual>
<origin xyz="0
${lr_reflect * (axle_length - (tire_width /
2 - hex_hub_depth))}
0"/>
<geometry>
<cylinder radius="${tire_dia / 2}" length="${tire_width}"/>
geometry>
<material name="tire_mat"/>
visual>
<collision>
<origin xyz="0
${lr_reflect * (axle_length - (tire_width /
2 - hex_hub_depth))}
0"/>
<geometry>
<cylinder radius="${tire_dia / 2}" length="${tire_width}"/>
geometry>
collision>
<xacro:thick_walled_tube_inertial
inner_rad="${hub_dia / 2}" outer_rad="${tire_dia / 2}"
height="${tire_width}" mass="${wheel_mass}"/>
link>
<gazebo reference="${lr_prefix}_${fr_prefix}_wheel">
<material>Gazebo/Blackmaterial>
gazebo>
xacro:macro>
这个wheel宏定义了轴架、轴和车轮,先创建了节点 $ { lr_prefix} _ ${fr_prefix} _ axle _ carrier,并以此为父节点创建了旋转形式的连接 $ {lr_prefix}_ $ { fr_prefix }_axle,创建了传动元件 $ {lr_prefix} _ ${fr_prefix} _ axle _trans" ,接着创建了节点 $ {lr_prefix} _ $ {fr_prefix} _ wheel,其余的内容在前面已经介绍过,大家类比着看就行了,就不介绍了
7、front_wheel
<xacro:macro name="front_wheel"
params="lr_prefix fr_prefix lr_reflect fr_reflect">
<xacro:shock lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}" fr_reflect="${fr_reflect}"
child="${lr_prefix}_steering_link"/>
<link name="${lr_prefix}_steering_link">
<xacro:null_inertial/>
link>
<joint name="${lr_prefix}_steering_joint" type="revolute">
<parent link="${lr_prefix}_steering_link"/>
<child link="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<axis xyz="0 0 1"/>
<limit lower="${-degrees_45}" upper="${degrees_45}"
effort="${servo_stall_torque}" velocity="${servo_no_load_speed}"/>
joint>
<transmission name="${lr_prefix}_steering_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${lr_prefix}_steering_joint">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
joint>
<actuator name="${lr_prefix}_steering_act">
<hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<xacro:wheel lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}"/>
xacro:macro>
这个“函数”,用来对前轮进行构建,调用了之前介绍过的函数shock函数,建立了节点 $ {lr_prefix} _ steering _ link ,以次为父节点建立了连接 $ {lr_prefix}_steering_joint,接着建立了传动元件$ { lr_prefix} _steering _trans,调用了“函数” wheel
8、Rear wheel
<xacro:macro name="rear_wheel"
params="lr_prefix fr_prefix lr_reflect fr_reflect">
<xacro:shock lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}" fr_reflect="${fr_reflect}"
child="${lr_prefix}_${fr_prefix}_axle_carrier"/>
<xacro:wheel lr_prefix="${lr_prefix}" fr_prefix="${fr_prefix}"
lr_reflect="${lr_reflect}"/>
xacro:macro>
这个“函数”,调用了“函数” wheel和shock
9、layer
<xacro:macro name="layer" params="layer parent m x y z *joint_pose">
<joint name="${layer}_joint" type="fixed">
<xacro:insert_block name="joint_pose"/>
<parent link="${parent}"/>
<child link="${layer}_link" />
joint>
<link name="${layer}_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${x} ${y} ${z}" />
geometry>
<material name="yellow" />
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${x} ${y} ${z}" />
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${x}" depth="${y}"
height="${z}" mass="${m}">
<origin xyz="0 0 ${-z / 2}"/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="${layer}_link">
<material>Gazebo/Orangematerial>
gazebo>
xacro:macro>
这个“函数”,根据传入的参数,创建了名为 $ {layer}_joint 的关节,其父节点为$ { parent },子节点为$ { layer } _ link,接着创建了这个子节点,最后对颜色进行了设定
第七部分:调用宏对阿克曼转向车模型进行构建
1、车主体部分的构建
<link name="base_footprint"/>
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.01"/>
geometry>
<material name="chassis_mat"/>
visual>
link>
<gazebo reference="base_link">
<material>Gazebo/Greymaterial>
gazebo>
<joint name="base_footprint_to_base_link" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0 0 ${(wheel_travel/2)+shock_z_offset}" />
joint>
<link name="chassis">
<visual>
<origin xyz="0 0 ${-chassis_height / 2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
geometry>
<material name="chassis_mat"/>
visual>
<collision>
<origin xyz="0 0 ${-chassis_height / 2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
geometry>
collision>
<xacro:solid_cuboid_inertial
width="${chassis_length}" depth="${chassis_width}"
height="${chassis_height}" mass="${chassis_mass}">
<origin xyz="0 0 ${-chassis_height / 2}"/>
xacro:solid_cuboid_inertial>
link>
<gazebo reference="chassis">
<material>Gazebo/Greymaterial>
gazebo>
<joint name="base_link_to_chassis" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
joint>
创建了节点base_footprint,接着创建的节点base_link,并且以前者为父节点,后者为子节点创建了固连关节base_footprint_to_base_link,接着创建了节点chassis,创建了关节base_link_to_chassis,其父节点为base_link,子节点为chassis
2、电池组的构建
<xacro:battery prefix="left" reflect="1"/>
<xacro:battery prefix="right" reflect="-1"/>
分别调用宏battery,构建了两个蓝色的电池组,宏battery内部结构前面已经介绍过了,就不展开了,画出以下结构图:
3、轮系的构建
<xacro:front_wheel lr_prefix="left" fr_prefix="front"
lr_reflect="1" fr_reflect="1"/>
<xacro:front_wheel lr_prefix="right" fr_prefix="front"
lr_reflect="-1" fr_reflect="1"/>
<xacro:rear_wheel lr_prefix="left" fr_prefix="rear"
lr_reflect="1" fr_reflect="-1"/>
<xacro:rear_wheel lr_prefix="right" fr_prefix="rear"
lr_reflect="-1" fr_reflect="-1"/>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<legacyModeNS>truelegacyModeNS>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
<robotNamespace>/racecarrobotNamespace>
plugin>
gazebo>
分别调用宏front_wheel和rear_wheel对四个轮子及其传动进行了构建了,宏内部结构前面已经介绍过了,就不展开了,轮系结构图较复杂,暂时先不展开了
4、传感器及其底板的构建
<layer layer="platform" parent="chassis" m="${layer_mass}" x="${layer_x}" y="${layer_y}" z="${layer_z}">
<origin xyz="0 0 ${layer_height}" rpy="0 0 0" />
layer>
<layer layer="lidar_platform" parent="platform_link" m="${layer_mass}" x="0.12" y="0.10" z="0.01">
<origin xyz="0.08 0 ${layer_height}" rpy="0 0 0" />
layer>
<joint name="lidar_joint" type="fixed">
<origin xyz="0 0 0.03" rpy="0 0 0" />
<parent link="lidar_platform_link"/>
<child link="laser_link"/>
joint>
<lidar prefix="laser"/>
<joint name="imu_joint" type="fixed">
<origin xyz="0.16 0 0.015" rpy="0 0 0" />
<parent link="platform_link"/>
<child link="imu_link"/>
joint>
<imu prefix="imu"/>
<joint name="camera_joint" type="fixed">
<origin xyz="0.16 0 0.05" rpy="0 0 0" />
<parent link="platform_link"/>
<child link="camera_link"/>
joint>
<camera prefix="camera"/>
robot>
调用宏layer创建了两个传感器底板platform_link和lidar_platform_link,并以lidar_platform_link为父节点,创建关节lidar_joint,其子节点为laser_link,调用了外部文件,创建了传感器lase,以platform_link为父节点,创建关节imu_joint和camera_joint,其子节点分别为imu_link、camera_link,调用了外部文件,创建了传感器imu、camera;除轮系结构外的结构如下(拖动图片,可在新打开的页面内查看大图):