xacro模型文件
xacro里面的模型仍然是urdf模型,但是从整个模型的管理上发生了很大的变化
作用:
1.精简模型代码
a、创建宏定义
b、文件包含
2.提供可编程接口
a、常量
b、变量
c、数学计算
d、条件语句
xacro使用方法
常量定义
<xacro:property name="M_PI" value="3.14159"/>
定义标签:xacro:property 后面跟两个参数属性 :1、name 2、value
name是想定义的常量名
value是常量值
常量使用
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
origin关键字;
使用常量 ${ } 在括号里使用常量 ,在括号里面可以进行运算
数学计算
<origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>
在括号里面可以进行运算
注意:所有数学运算都会转换成浮点数进行,以保证运算精度
宏定义
<xacro:macro name="name" params="A B C">
......具体模型定义(类似函数内容)
</xacro:macro>
定义标签:xacro:macro 后面跟两个参数属性 :1、name 2、params
name:宏定义的名字类似函数名
params:类似函数参数,可以是字符串
宏调用
<name A="A_value" B="B_value" C="C_value" />
文件包含
<xacro:include filename="$(find mbot_descripiton)/urdf/xacro/mbot_base.xacro" />
定义标签:xacro:include
$(find+功能包)=包的具体路径
模型显示
1.方法一:(不常用)
将xacro文件转化成URDF文件后显示
$rosrun xacro xacro.py mbot.xacro>mbot.urdf
2.方法二: (常用)
直接调用xacro文件解析器
<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot.xacro'" />
<arg name="gui" default="true" />
<param name="robot_description" command="$(arg model)" />
实战解读代码
<?xml version="1.0" ?>
<robot name="fourwheelrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--Defineing namespace-->
<!--property list --> <!--常量定义-->
<xacro:property name="M_PI" value="3.141592"/>
<xacro:property name="base_mass" value="20" />
<xacro:property name="base_length" value="0.80"/>
<xacro:property name="base_width" value="0.550"/>
<xacro:property name="base_height" value="0.30"/>
<xacro:property name="wheel_mass" value="2" />
<xacro:property name="wheel_radius" value="0.122"/>
<xacro:property name="wheel_length" value="0.062"/>
<xacro:property name="wheel_joint_x" value="0.25"/>
<xacro:property name="wheel_joint_y" value="0.306"/>
<xacro:property name="wheel_joint_z" value="-0.1"/>
<!--Defineing the colors--> <!--颜色定义-->
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
<!-- Macro for inertia matrix --> <!--宏定义了惯性矩阵-->
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" /> <!--由于惯性矩阵以对角线对称的-->
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
<xacro:macro name="box_inertial_matrix" params="m a b c">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(b*b+c*c)/12}" ixy = "0" ixz = "0"
iyy="${m*(c*c+a*a)/12}" iyz = "0"
izz="${m*(a*a+b*b)/12}" />
</inertial>
</xacro:macro>
<!--Macro for the wheel --> <!--定义轮子-->
<xacro:macro name="wheel" params="prefix1 prefix2 reflect1 reflect2">
<joint name="${prefix1}_${prefix2}_wheel_joint" type="continuous">
<origin xyz="${reflect1*wheel_joint_x} ${reflect2*wheel_joint_y} ${wheel_joint_z}"/>
<parent link="base_link"/>
<child link="${prefix1}_${prefix2}_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix1}_${prefix2}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix1}_${prefix2}_wheel_link">
<material>Gazebo/Black</material>
</gazebo>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix1}_${prefix2}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="prefix1}_${prefix2}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix1}_${prefix2}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!--start -->
<xacro:macro name="fourwheelrobot_base_gazebo">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rqy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${base_height/2}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<origin xyz="0 0 0" rqy="0 0 0"/>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="yellow"/>
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<box_inertial_matrix m="${base_mass}" a="${base_length}" b="${base_width}" c="${base_height}" />
</link>
<gazebo reference="base_link">
<material>Gazebo/Yellow</material>
</gazebo>
<wheel prefix1="left" prefix2="front" reflect1="1" reflect2="1"/>
<wheel prefix1="left" prefix2="back" reflect1="-1" reflect2="1"/>
<wheel prefix1="right" prefix2="front" reflect1="1" reflect2="-1"/>
<wheel prefix1="right" prefix2="back" reflect1="-1" reflect2="-1"/>