复制代码
<?xml version="1.0" encoding="UTF-8"?>
<robot name="shape_inertial_demo" xmlns:xacro="http://ros.org/wiki/xacro">
<link name="base_footprint"/>
<joint name="base_footprint_to_base_link" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
</joint>
<!-- ==================== 1. 立方体连杆（base_box）==================== -->
<link name="base_link">
<!--
origin xyz="0 0 0.1" rpy="0 0 0" 这是相对于 name="base_link"的位姿
对于urdf 原生支持的 box cylinder sphere来讲
这个值是指定了。。。。几何中心。。。。的位姿
在实际过程中 visual collision inertial 的xyz rpy 一般应该是一致的
否则会导致视觉和物理效果的错乱
比如 collision 和visual不匹配会导致
明明已经产生了碰撞 但是看起来没有
或者已经穿模了 还在继续往前走
比如 inertial 和visual不匹配会导致
看着放的稳稳当当 但实际就是东倒西歪
-->
<visual>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<geometry>
<box size="0.4 0.4 0.2"/> <!-- l_x=0.4, l_y=0.4, l_z=0.2 -->
</geometry>
<material name="blue">
<color rgba="0.2 0.2 0.8 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<geometry>
<box size="0.4 0.4 0.2"/>
</geometry>
</collision>
<inertial>
<mass value="4.0"/> <!-- 质量m=4kg -->
<origin xyz="0 0 0.1" rpy="0 0 0"/> <!-- 几何中心=重心，与link原点重合 -->
<!-- 立方体惯性张量公式：
ixx = 1/12 * m * (l_y² + l_z²)
iyy = 1/12 * m * (l_x² + l_z²)
izz = 1/12 * m * (l_x² + l_y²)
代入参数：m=4, l_x=0.4, l_y=0.4, l_z=0.2
计算过程：
ixx = 1/12 * 4 * (0.4² + 0.2²) = (4/12) * (0.16 + 0.04) = (1/3) * 0.2 ≈ 0.0667
iyy = 1/12 * 4 * (0.4² + 0.2²) = 0.0667（与ixx相等，因l_x=l_y）
izz = 1/12 * 4 * (0.4² + 0.4²) = (4/12) * (0.16 + 0.16) = (1/3) * 0.32 ≈ 0.1067
-->
<inertia ixx="0.0667" ixy="0.0" ixz="0.0"
iyy="0.0667" iyz="0.0"
izz="0.1067"/>
</inertial>
</link>
<!-- ==================== Gazebo颜色配置（核心）==================== -->
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<!-- ==================== 关节定义 ===================== -->
<!-- 关节1：base_box → cylinder_link（绕Z轴旋转） -->
<joint name="box_to_cylinder" type="revolute">
<origin xyz="0 0 0.2" rpy="0 0 0"/> <!-- 圆柱体连接在立方体顶面中心 -->
<parent link="base_link"/>
<child link="cylinder_link"/>
<axis xyz="0 0 1"/> <!-- 绕Z轴旋转 -->
<limit effort="50" lower="-3.14" upper="3.14" velocity="1.0"/>
</joint>
<!-- ==================== 2. 圆柱体连杆（cylinder_link）==================== -->
<link name="cylinder_link">
<visual>
<origin xyz="0 0 0.15" rpy="0 0 0"/> <!-- 几何中心在link原点 -->
<geometry>
<cylinder length="0.3" radius="0.1"/> <!-- r=0.1m，l=0.3m（沿Z轴） -->
</geometry>
<material name="gold">
<color rgba="0.96 0.76 0.13 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.15" rpy="0 0 0"/>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
</collision>
<inertial>
<mass value="1.0"/> <!-- 质量m=1kg -->
<origin xyz="0 0 0.15" rpy="0 0 0"/> <!-- 几何中心=重心 -->
<!-- 圆柱体惯性张量公式（轴线沿Z轴）：
ixx = iyy = 1/12 * m * (3r² + l²)
izz = 1/2 * m * r²
代入参数：m=1, r=0.1, l=0.3
计算过程：
ixx = iyy = 1/12 * 1 * (3*0.1² + 0.3²) = (1/12)*(0.03 + 0.09) = 0.12/12 = 0.01
izz = 1/2 * 1 * 0.1² = 0.5 * 0.01 = 0.005
-->
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.005"/>
</inertial>
</link>
<gazebo reference="cylinder_link">
<material>Gazebo/Gold</material>
</gazebo>
<!-- 关节2：cylinder_link → sphere_link（固定） base_box -->
<joint name="cylinder_to_sphere" type="fixed">
<origin xyz="0 0 0.3" rpy="0 0 0"/> <!-- 球体连接在圆柱体顶端中心 -->
<parent link="cylinder_link"/>
<child link="sphere_link"/>
</joint>
<!-- ==================== 3. 球体连杆（sphere_link）==================== -->
<link name="sphere_link">
<visual>
<origin xyz="0 0 0.1" rpy="0 0 0"/> <!-- 球心=link原点 -->
<geometry>
<sphere radius="0.1"/> <!-- r=0.1m -->
</geometry>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
<geometry>
<sphere radius="0.1"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/> <!-- 质量m=0.5kg -->
<origin xyz="0 0 0.1" rpy="0 0 0"/> <!-- 球心=重心 -->
<!-- 球体惯性张量公式：
ixx = iyy = izz = 2/5 * m * r²
代入参数：m=0.5, r=0.1
计算过程：
ixx=iyy=izz = 2/5 * 0.5 * 0.1² = 0.2 * 0.01 = 0.002
-->
<inertia ixx="0.002" ixy="0.0" ixz="0.0"
iyy="0.002" iyz="0.0"
izz="0.002"/>
</inertial>
</link>
<gazebo reference="sphere_link">
<material>Gazebo/Red</material>
</gazebo>
</robot>